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Abstract 


This Paper explores the use of an extended Kalman filter to provide real-time esti- 
mates of underwater vehicle position and attitude. The types of previously available sen- 
sors are detailed including strapdown accelerometers, roll and pitch sensors, gyro and 
magnetic compasses, depth sensor, and various types of acoustic positioning systems. A 
doppler velocimeter is added to this sensor suite to improve the performance of the filter. 
As an integral part of the filter, magnetic compass and gyrocompass biases are estimated 
to improve vehicle heading accuracy. The filter is designed to account for numerous real- 
life complications. These include varying rates of sensor output, lengthy gaps in reception 
of position information, presence of non-Gaussian position fix errors (flyers), and varying 
probability density functions for sensor errors. Simulated data are used to test the filter 
with varying availability of data and accuracy of initial conditions, along with actual data 
from a deployment of the towed DSL-120 vehicle. The increased accuracy obtained by 
using the doppler velocimeter is emphasized. 


Thesis Supervisor: Dr. W. Kenneth Stewart, Jr. 
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Chapter 1 


Introduction 


1.1 Motivation 


This thesis has two major goals. The first is to provide accurate estimates of 
underwater vehicle position and attitude and thereby improve the quality of sonar and 


video information collected. The second is to provide these estimates in real-time. 


1.1.1 Improving Vehicle Position and Attitude Information 


More than two thirds of the Earth is covered by water. Until very recently in 
history, the ocean depths were mysterious and inaccessible. However, in the past few 
decades new technologies have dramatically expanded man’s abilities to explore the ocean 
bottom. Discoveries range from the remnants of human incursions, epitomized by the 
Titanic, to new life on the ocean floor gaining its sustenance from the earth’s interior 
rather than the sun. 

One of the primary tools for conducting underwater searches and surveys from 
unmanned vehicles is the sidescan sonar. The sonar emits a beam of relatively high- 
frequency sound on either side of the vehicle. When this sound beam hits the bottom or 
any objects within range, it is reflected and the return is received by transducers mounted 
on the vehicle. The characteristics of the received sound, primarily magnitude and phase, 
can then be analyzed to determine the characteristics and contours of the bottom as well as 
the location and general shape of any objects. 

For this system to work at an optimum level, itis vital that the location and attitude 
of the vehicle be known as accurately as possible. Any position errors lead directly and 
obviously to an error in the assumed position of all objects mapped by the sonar. Equally 
important, however, is knowledge of the vehicle's roll, pitch, and heading. Due to the 
nature of sound propagation in water, a significant time lag occurs between the 


transmission of a sonar ping and the receipt of any returns. During this time, the vehicle 


will not only have changed position but may have changed its attitude as well. For proper 
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interpretation of the sonar returns, the attitude of the vehicle at the time of sonar 
transmission must be known, because that determines the precise direction of the beam. 
This same information is also needed at the time of the sonar return, so the location of any 


objects found can be accurately determined. 


1.1.2 Previous Work on Underwater Vehicle Navigation 


The invention of feasible inertial navigation produced a great interest in combining 
inertial measurements with other fix sources to improve navigational accuracy. These 
early inertial navigation units required gimbal-stabilized platforms to achieve the desired 
accuracy, which added size and weight. Due to these restrictions, early work was confined 
to aircraft and ships, including submarines. In the early 1970s, the U.S. Air Force was one 
of the first to evaluate the use of the Kalman filter to integrate inertial measurements with 
external fix sources (D’Appolito, 1971). 

Later, strapdown inertial systems permitted much smaller and lighter equipment, 
though at a cost in accuracy. Once again, the military provided a major impetus to 
developing these systems to their fullest potential. For example, the North Atlantic Treaty 
Organization’s (NATO) Advisory Group for Aerospace Research and Development 
(AGARD) included much work on these strapdown systems, although still for use 
primarily in aircraft and spacecraft (VanBronkhorst, 1978; Catford, 1978). 

Ever since remotely operated vehicles (ROVs) became feasible, there has been 
interest in improving their navigational accuracy as well. Early ROVs had only a few 
sensors available to provide navigational information, since inertial navigational systems 
were still too heavy and power-intensive to be usable. These included a magnetic compass 
to provide heading and inclinometers to measure pitch and roll. Additionally, an acoustic 
transponder network was often used to provide position information for operations in a 
small area. A later improvement was the gyrocompass, which provides a more consistent 
output than the magnetic compass but adds new errors caused by drift. 

Improvements in the information provided by these acoustic transponder nets has 
been marked. In her 1992 thesis, Diane Di Massa provided one example with her 
exploration of the possibilities of hyperbolic navigation to permit long-range acoustic 


navigation by an Autonomous Underwater Vehicle (AUV) (Di Massa, 1992). Brian 





Tracey contributed another with his improvements to a newer technique for ROV 
navigation, ultrashort-baseline navigation, in his 1992 thesis (Tracey, 1992). 

In the past few years, a number of new devices have been developed to assist in 
solving this navigational problem. In his 1988 thesis, Gregory Vaughn discussed the use of 
a lightweight, strapdown inertial motion unit in conjunction with an external acoustic 
positioning net to improve closed-loop vehicle control (Vaughn, 1988). He concluded that 
by incorporating this new data into a Kalman filter to provide real-time optimal estimates 
of vehicle parameters, closed-loop control of ROVs, such as the Woods Hole 
Oceanographic Institution’s Jason, could be improved. However, for his application 
vehicle attitude was unimportant and therefore was not incorporated into his simulations. 

A fairly recent addition to the repertoire of instruments available to the 
oceanographic engineer is the doppler velocimeter, also known as the acoustic doppler 
current profiler (ADCP). For the first time, accurate real-time velocity measurements of a 
ROV could be made relatively cheaply and without exceeding the restrictive power and 
weight limits inherent in ROV operations. RD Instruments, a doppler velocimeter 
manufacturer, has been funded by the Office of Naval Research to explore the use of the 
instrument to provide an independent and accurate measurement of vehicle velocity 
within the context of a Kalman filter. Their initial work verifies the utility of the 
velocimeter, although so far their research has been confined to inertial navigation systems 
too large for ROV use (Rowe and Brumley, 1992). 

These are but a few examples of the extensive work that has been published on 
improving underwater vehicle navigation. A search of the literature reveals diverse 
techniques for improving sensor information and integration, including several variations 


on Kalman filtering techniques. 


1.1.3 Cost and Processing Time Reduction 


In the real world, cost considerations are almost always important. Cruises are 
expensive and usually result in computer disks full of unprocessed, raw data, which are 
essentially worthless until processed. This processing can take many man hours, which 
translates into a considerable cost. Unfortunately, this sometimes means that processing is 


never completed, since either the time or the money runs out. Therefore, the more real- 





time processing that can be performed the better. Real-time processing means that the 
important data are immediately available. Also, since it is accomplished at sea while the 
cruise 15 in progress (and already paid for), additional expenses are minimized. 

Another important application for this capability pertains to autonomous 
underwater vehicles (AUVs). Much research is being conducted currently on these 
vehicles, which should be capable of operating independently for days or even weeks. 
Real-time processing is essential to permit the vehicle to navigate successfully and is also 


vital in reducing the on-board data storage requirements of AUV deployments. 


1.1.4 Resources Available 


This thesis invesugates how the navigational sensors currently available at the 
Deep Submergence Laboratory (DSL) can be used to improve the real-time position and 
attitude information available for the operation of a ROV. This summer, DSL personnel 
participated in a cruise aboard the R/V Knorr. A major purpose of the cruise was to obtain 
detailed sidescan sonar data from an area of the Mid-Atlantic Ridge where seafloor 
spreading is occurring. The DSL-120, a towed vehicle carrying a 120-kHz sidescan sonar 
system and multiple navigational sensors, was used for this survey. Data from these DSL- 
120 deployments are used to demonstrate the performance of the Kalman filter developed 
in this thesis. By integrating the velocity data provided by the doppler velocimeter into a 
Kalman filter algorithm, the vehicle’s navigational accuracy can be greatly enhanced. This 
substantially improves the usefulness of the sonar data collected and demonstrates the 


feasibility of performing real-time navigation sensor processing. 


1.2 Advantages and Limitations of the Kalman Filter 


Sonar data collected by underwater vehicles has often been rendered nearly useless 
due to inaccurate navigation information and an overly simplistic approach to tracking 
vehicle parameters. For example, relying solely on a magnetic compass for heading 
information nearly guarantees that the measured heading will be inaccurate. 

The Kalman filter uses a state space model to provide a well defined method for 


integrating the information obtained from different sensors into a coherent whole. Taking 





advantage of the a priori knowledge of the relative accuracies of these sensors as well as 
the dynamics of the system, the Kalman filter can provide real-time output without 
overstressing Computer memory or computational limits. 

The addition of the doppler velocimeter provides a marked improvement in the 
ability of a Kalman filter to provide valid position and attitude information. By providing 
accurate measurements of the magnitude of vehicle velocities in all three dimensions, a 
more precise, Continuous estimate of vehicle position and attitude can be provided than 
would be available by the other sensors alone. 

In this application, there are several obstacles to overcome in creating a usable 
Kalman filter. First, the system is nonlinear and cannot therefore use the simplest and most 
mathematically precise version of Kalman filter theory. Instead, an extended Kalman filter 
is required. Second, a method for ignoring obviously faulty measurements must be 
included. Finally, there are several practical problems involved in using the output of such 
a wide variety of sensors. These problems and their proposed solutions are discussed in 


detail later in this thesis. 


1.3 Thesis Outline 


Chapter Two describes the sensors used to provide information for the Kalman 
filter. Principles of operation for each sensor are discussed, as well as limitations and 
considerations for each when integrated into the overall system. 

Chapter Three explains the development of the Kalman filter and its state space 
model for this application. A basic review of Kalman filter theory is included, with 
emphasis on the specifics of the extended Kalman filter required for nonlinear problems. 
This chapter also discusses the specific problems that were addressed for proper filter 
operation and how the filter was implemented. 

Chapter Four shows the results of filter operation by using both simulated and 
actual data. The improvement in performance obtained by using the doppler velocimeter 


measurements of vehicle velocities is emphasized. 





The results are summarized in Chapter Five, along with suggestions for further 
refinements to the filter. Finally, some possibilities for additional experiments are 


discussed. 
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Chapter 2 


Sensor Information 


2.1 Overview of Sensors 


Like any mathematical abstraction of a real-world system, the Kalman filter is a 
simplified model of reality. For the filter to function optimally, the model must be as 
complete and as accurate as possible. For this application there are two major aspects of 
the filter that must be considered. The first concerns prediction: based on knowledge of 
present system parameters, what will happen to these parameters in the future? The second 
is more basic: how can knowledge of these system parameters be obtained? 

In this chapter, the latter issue is addressed. Each sensor providing input to the 
filter is described in detail. Using our knowledge of the available sensors, the Kalman 
filter model can then be developed. Before proceeding further, however, a clarification of 


the coordinate systems used is required. 


2.2 Coordinate Systems 


Within the filter algorithm, two different coordinate systems are used. The first is 
earth-referenced, with positions measured relative to fixed reference points on the earth. 
The second is vehicle-referenced, also referred to as body-referenced. In this case, all 
measurements are made relative to a specific point on the ROV, usually the center of 
rotation of the vehicle. Table 2-1 and Figure 2-1 explain the differences in more detail. 

An additional coordinate system is sensor-referenced, which depends on the 
location of the individual sensor. For linear acceleration and velocity measurements, a 
difference in measurement magnitudes between sensor-referenced and vehicle-referenced 
measurements exists due to the effects of angular velocities when the sensor is mounted 
away from the vehicle's center of rotation. The orientation of the axes is the same as 


vehicle-referenced coordinates since the instruments are mounted colinear with the 


vehicle axes. Compensation for this difference is discussed in Section 3.5.3. 
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Figure 2-1. Vehicle-referenced coordinate system. 


Table 2-1: Comparison of Coordinate Systems 


T Earth-Referenced | Vehicle-Referenced 


Longitude (+X = east) Perpendicular to side of 
vehicle (+X = to starboard) 


Caittude (tY = noni) In direction of vehicle bow 
or stern (+Y = forward) 

Z Depth/Altitude (+Z = up) | Perpendicular to top or bot- 
tom of vehicle (+Z = up) 


2.3 Doppler Velocimeter 






The proper name for this instrument, used to provide vehicle velocities, 1s the 


Direct-Reading Broadband Acoustic Doppler Current Profiler (DR-BBADCP). 


Manufactured by RD Instruments (RDI), the DR-BBADCP ts designed to measure current 


velocities at discrete points through the water column (see Fig. 2-2). Its alternate mode, 


which is the mode used to gather data for this application, is the bottom-track profiling 


mode. With the DR-BBADCP mounted on the vehicle looking downward, vehicle 
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Figure 2-2. Broadband ADCP setup for current profiling (RDI, 1993). 


instrument in this application, the more descriptive name of “doppler velocimeter” is used 
in the remainder of this thesis. 


2.3.1 Principles of Operation 


The doppler velocimeter uses four downward-looking acoustic beams operating at 
high frequency. Figure 2-3 shows the beam pattern of two of these beams tn an upward- 


looking mode. RDI manufactures these instruments with six different transmit 
frequencies: 75, 150, 300, 600, 1200, and 2400 kHz. As with any sonar, increasing 


frequency improves the accuracy but reduces the effective range. Therefore, the frequency 
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Fig. 2-3. ADCP beam geometry (RDI, 1993). 


for a given operation ts chosen to be as high as possible while still providing sufficient 
range to ensonify the bottom based on the expected altitude of the vehicle. 

As evidenced by its name, the instrument operates on the doppler principle. 
Vehicle velocity in the direction of a beam increases the frequency of the returned signal, 
while velocity away from the beam decreases it. Specifically, the vehicle velocity in the 


direction of the beam is calculated by (RDI, 1993). 


Relative Flow Velocity (m/s) = Ep x 2E. 


where 


Fy is the measured doppler frequency shift in kHz, 
c is the speed of sound in water at the transducer face in m/s, 


Fe is the transmitted acoustic frequency in kHz. 
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Fig. 2-4. Frequency change caused by doppler effect (RD/, 1993). 


Figure 2-4 shows this effect with a fixed transducer and moving material in the 
water. The effect is the same with the transducer mounted on a moving vehicle over the 
fixed bottom. The four beams transmit at different angles so the velocities in each 
direction can be resolved. Three are needed to solve for velocities in all three directions 
and the fourth provides redundancy to ensure accuracy. 

To interpret the received sonar data, the doppler velocimeter uses an 
autocorrelation processor. A series of short pulses is transmitted, each with a pulse length 
of Tp with a known lag Tj, between pulses. When the returns are received, the processor 
compares the phase change between two distinct pulses, accounting for the difference in 
time between their respective transmissions. As shown in Fig. 2-5, a zero phase change 
implies zero velocity. Likewise, a phase change equates to a velocity with magnitude 
determined by the amount of phase change. Phase changes with a magnitude greater than 


2n are resolved using a proprietary RDI algorithm using different subsets of the ~ 
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Fig. 2-5. Phase change seen by the broadband system (RDI, 1993). 


transmitted pulses. If any datum is below the operator-selector minimum for correlation 
magnitude, that datum is rejected by the doppler velocimeter’s processor. 

The output of the velocimeter can be referenced four different ways. The most 
basic provides measurements of the velocities relative to each of the four beams. 
Alternatively, the velocimeter’s internal processor can resolve the components of the four 


beams to provide velocity fore or aft, velocity left or right, and velocity up or down 


21 





relative to the instrument. Third, if the instrument is not mounted on a ship or ROV with 
the same orientation as the platform’s reference axes, the velocities can be converted to 
any desired direction relative to the platform using knowledge of the difference between 
the instrument axes and platform axes. Finally, the doppler velocimeter can use its internal 
flux-gate sensor, which uses heading and pendulum sensors to provide pitch and roll. The 
internal processor can use this roll, pitch, and heading information or similar data from 
external sensors to convert from vehicle-referenced to earth-referenced velocities. 

For this Kalman filter the velocimeter is mounted with axes oriented the same as 
the vehicle axes described in Section 2.2. Therefore, a coordinate transformation to the 
platform axes is unnecessary except for any correction due to angular velocities. Instead of 
allowing the velocimeter's internal processor to convert velocities to earth-referenced 
coordinates, which would use the current output of the appropriate sensor for attitude 
measurements, the filter uses the vehicle-referenced values. Using vehicle-referenced 
velocities allows the filtered estimates of roll, pitch, and heading to be used to make the 
conversion to earth-referenced coordinates. This improves overall filter performance since 


the filtered estimates are less noisy than the instantaneous sensor outputs. 


2.3.2 Inputs to Velocimeter 


To provide the desired output, the doppler velocimeter uses the speed of sound in 
its calculations of vehicle velocity. As is the case for data used in this thesis, this speed of 
sound is typically entered manually by the operator to permit easier post-processing of the 
data. However, for more accurate calculations the doppler velocimeter can calculate the 
current speed of sound using three inputs: salinity, depth, and temperature. 

For most open-ocean operations salinity is constant. Therefore, salinity can be 
manually inserted prior to operations. If operations are to be conducted in an area of 
varying salinity, such as near a river mouth, then an external conductivity sensor can be 
used to provide salinity information to the velocimeter. 

Since the majority of vehicle operations occur within a relatively narrow depth 
band, making speed of sound changes due to depth change negligible, depth IS also 


normally manually inserted. If operations involving large depth changes are expected, 
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consideration should be given to providing depth estimates to the velocimeter to calculate 
a variable speed of sound. 

Finally, the doppler velocimeter has an internal temperature sensor. Manual input 
can be used in this case as well. For operations near the bottom in deep water, where 
thermal gradients are negligible, manually inputting temperature permits more precise 
post-processing of the data without introducing significant errors. In areas with larger 


gradients, using the installed temperature sensor is necessary. 


2.3.3 Doppler Velocimeter Outputs 


Obviously, the primary output desired from the doppler velocimeter is vehicle- 
referenced velocity. However, there are several operator-selected options that affect the 
accuracy and the frequency of these measurements. 

Frequency of output is determined by three factors: the speed of sound in the 
water, the range to the bottom (vehicle altitude), and the number of pings per ensemble. 
The doppler velocimeter averages the results from the pings in each ensemble to improve 
accuracy. The technical manual recommends four pings per ensemble. A larger number of 
pings per ensemble results in better quality measurements but at the cost of a lower update 
rate. Using the recommended four pings per ensemble, an average altitude of 100m, and 
the nominal speed of sound in seawater of 1500 m/s, the time between velocity 


measurements can be calculated as 
4 pings x 200 m/ping x 1 s/1500 m = 0.53 s/ensemble. 


The standard deviation for velocity measurements is computed by the 
velocimeter's processor based on the BBADCP frequency (for the model in use), the 
range to the bottom, the number of pings per ensemble, and the size of the depth cells 
selected prior to operation. The size of the depth cell is important for precision when 
operating in the water column profiling mode. For bottom tracking, however, the largest 
depth cell should always be used since this gives the lowest standard deviation for vehicle 
velocity measurements. For vehicle operations treated in this thesis, the standard deviation 
is on the order of 1-2 cm/s. Figure 2-6 shows how accuracy varies with range for a single 


ping using the 150-kHz model. This high accuracy enables precise measurement of 
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Fig. 2-6. Standard deviation for a single ping using the 150-kHz systein (with ADCP 
velocity, from top to bottom, of 20 knots, 10 knots, and 0 knots) (RD/, 1993). 


velocity magnitude. However, overall system accuracy 1s limited by how well heading, 
pitch, and roll can be estimated. Since standard deviation is constantly updated by the 
doppler velocimeter’s software, that information can be included in the Kalman filter 


algorithm. Specifics of this implementation are discussed in Chapter 3. 


2.4 Depth Sensor 


The depth sensor used for this work is manufactured by Paroscientific, Inc. and 
consists of a quartz-crystal resonator whose frequency of oscillation varies with pressure- 
induced stress. The sensor also includes thermal compensation using a quartz-crystal 
temperature signal. Nominal accuracy of the sensor is 0.02%, and a reading can be 
obtained approximately every 0.25s. (Paroscientific, 1987). 

For the purposes of achieving the best overall estimates of vehicle position and 
velocity, the absolute error of this sensor is less important than its stability. Previous 
experience indicates that the depth sensor should be very consistent in its outputs. 


Therefore, the output of the sensor is a vital element in increasing the accuracy of the 





Kalman filter used to determine state space estimates for depth-dependent vehicle 
parameters. 

The only time the sensor should exhibit less stability than desired is if the sensor is 
not in thermal equilibrium with the surrounding water. This could occur soon after initial 
deployment. However, in this case the problem would be foreseen, and thermal 
equilibrium could be achieved prior to commencing actual search operations. Of more 
concern is operation in water with strong thermal gradients. This problem could be 
particularly acute in shallow-water operations, especially at certain times of the day. 
During such operations, the pressure sensor can be expected to give less accurate results, 


resulting in greater Kalman filter errors. 


2.5 External Positioning System 


There are several possible systems that can be used to provide external position 
information. Historically, nearly all underwater vehicle positioning information has been 
obtained from some type of acoustic net. These acoustic systems generally fall into three 
different categories, with the type used for a specific operation dependent on the type of 


mission and the topography of the bottom. 


2.5.1 Long-Baseline Systems 


In a long-baseline system, an array of acoustic transponders is deployed on the 
bottom in the vicinity of projected vehicle operations. A transponder is also attached to the 
vehicle. Vehicle position is determined by measuring the travel times of sound waves 
between the vehicle and the transponders in the net. For two-way systems, the vehicle 
sends out a ping, and each transponder responds with a coded return signal when it 
receives the initial ping from the vehicle. The travel times are then translated into range. 
One-way systems use the same principle, but travel time 1s measured only from the 
vehicle to each transponder in the net. With either system, since the positions of the 
transponders on the bottom have already been determined when calibrating the acoustic 


A 


net, the position of the vehicle can be determined (Morgan, 1978). 
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An example of such a system is the Sonic High Accuracy Ranging and Positioning 
System (SHARPS). This commercial system uses three transceivers near the ocean 
bottom, which are connected to a surface ship by coaxial cables. Another transceiver is 
mounted on the ROV, which is also connected to the ship. Because the cable connections 
allow the surface ship to know the exact transmission time of each ping, only one-way 
travel times are needed, which allows twice the frequency of updates as a two-way 
system. Operating at 300 kHz, SHARPS is limited to a range of 100 m in seawater but can 
provide accuracies on the order of 2 cm (Somers, 1991). 

A disadvantage of SHARPS is the need for multiple cables from the surface ship to 
the transponder field, which can cause difficulties when maneuvering a ROV in the area. 
Another system, EXACT, was used by Yoerger and Mindell to control the ROV Jason in 
the vicinity of an active hydrothermal plume at a depth of 2200 meters. The EXACT 
system operates similarly to SHARPS and provides comparable accuracies but uses no 
cable connections to the surface. Instead, the master unit on the ROV communicates with 
the surface through a low-band width serial link. Because there are no cable connections, 
two-way travel times are used (Yoerger and Mindell, 1992). 

The major advantages of a long-baseline system are its stability and its accuracy 
when conducting vehicle operations in deep water close to the bottom. Because the 
transponder array is on the seafloor, it is fixed once placed and calibrated. Therefore, its 
accuracy is improved over a less stable system. Since the array of transponders is 
deployed on the bottom near the area of operations, the difference in acoustic travel times 
between each pinger and the vehicle will be greater than if all pingers were located close 
to the surface, which also enhances accuracy. Finally, the acoustic net can be made as 
large as desired. By using lower frequency systems, the maximum range to the ROV can 
be greatly increased at the cost of reduced accuracy and decreased update trequency. 

There are two major disadvantages of this method. First, because a transponder 
net must be deployed on the bottom and calibrated, significant preparation time is required 
before ROV operations can begin. Second, vehicle operations are restricted to the vicinity 
of the acoustic transponders. Therefore, for large-area surveys, this method is impracucal. 
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In this case, a short-baseline system may be used (Morgan, 1978). 





2.5.2 Short-Baseline Systems 


The principles of operation of the short-baseline systems are similar to those 
having a long baseline. In fact, a system such as SHARPS can be used in either 
configuration. The obvious difference ts that instead of the acoustic net transponders being 
placed on the ocean floor, they are attached to the surface ship. This method eliminates 
some disadvantages of the long-baseline system. Since the transponders are mounted on 
the surface vessel in known locations, no initial surveying is required. Also, the ship can 
be moved wherever operations are desired, so the range limitations of a bottom-fixed net 
are negated. 

Like the long-baseline system, the short-baseline system measures the difference 
in arrival time from the acoustic pinger on the ROV to each transponder on the ship. 
However, because the geometry of the ship-mounted array is precisely known, this 
difference in arrival times can be converted into an angular direction from the plane of the 
array to the ROV. Since the ROV is tethered to the ship, the time of each ping is also 
known precisely. Therefore, both bearing and range information are available, yielding the 
location of the ROV (Morgan, 1978). 

However, the short-baseline system tends to increase position errors because of the 
more sensitive geometry. Also, because the transponder array is now mounted on a 
moving platform, surface positioning errors can become the dominant factor in 
determining vehicle location. The importance of accurately measuring the surface ship's 
heading, pitch, and roll also become vital, especially when the range to the ROV 
increases, since errors in these measurements result in an erroneous measurement of 


bearing to the ROV (Somers, 1991). 


2.5.3 Ultrashort-Baseline Systems 


The ultrashort-baseline system uses a hydrophone array mounted either on the 
ROV or the surface ship, with only a single pinger on the other platform. Normally, the 
hydrophone array is on the surface ship. Phase comparisons rather than travel-time 
measurements are used to determine the position of the vehicle. The acoustic pinger sends 


out a pulse, and the processor connected to the hydrophone array measures the phase 





difference in the signals received at each hydrophone. This phase difference is converted 
into a bearing to the pinger. For tethered vehicles, the time of travel can also be computed 
as in the short-baseline case to obtain range. Therefore, ROV position can be determined. 
To measure phase difference accurately, the receiving hydrophones are mounted within 
one wavelength of each other (typically, only centimeters apart), making the array small 
enough for use on ROVs. 

This system has two major disadvantages. First, the installation must be extremely 
precise, which can be difficult. Second, multipath arrivals of the sound waves via bottom- 
bounce or surface-reflection can cause false phase measurements resulting in large 
inaccuracies. In his 1992 thesis, Brian Tracey explores techniques to minimize the effects 


of this multipath interference (Tracey, 1992). 


2.5.4 Other Positioning Systems 


Besides traditional acoustic positioning systems, other possibilities for 
determining vehicle position exist. For example, research is being performed at DSL and 
other laboratories in making terrain-relative mapping feasible. This method entails 
matching bottom features seen by the vehicle with known features from an existing 
database, or with features already found and mapped by the vehicle. Depending on the 
features present in the vicinity, the quality of the database, and such system characteristics 


as sonar frequency, position fixes with accuracies on the order of meters are foreseeable. 


2.5.5 Position Information Required for Use in the Kalman Filter 


For the purposes of constructing the filter, any of these system configurations can 
be used, but allowances must be made to account for the differences between them. A 
longer range system, using lower frequencies, is typically less accurate. Additionally, 
since the frequency of fixes is limited by the speed of sound in the water, the longer range 


system will obtain fixes less frequently. The filter must be adjusted accordingly. 





2.6 Gyro 


The gyro used at DSL is manufactured by Humphrey, Inc. It is a standard 
uncompensated model that provides continuous heading information. It maintains 
excellent short-term precision with little “jitter,” or high-frequency fluctuation. However, 
because it is uncompensated, it has a high drift rate that tends to fluctuate from three to six 
degrees per hour. This drift introduces a significant and variable bias that can have a 
debilitating effect on the Kalman filter, which assumes only zero-mean Gaussian errors. 
To correct for this bias, it is included in the filter state space so that it can be continuously 


adjusted as necessary based on other heading measurements. 


2.7 Flux-Gate Magnetic Compass 


The magnetic compass used at the DSL is the C-100, manufactured by KVH 
Industries, Inc. It consists of a toroidal flux-gate sensing element with an associated 
electronics board. The sensor element is a saturable ring core, which floats in an inert fluid 
so the sensing element remains horizontal. Windings surround the lexan housing of the 
sensor element, electrically driving the coil into saturation. Secondary windings then 
sense pulses generated by the horizontal component of the earth's magnetic field (KVH, 
No | 

The compass provides heading information at 10 Hz. The manufacturer's 
specifications list the accuracy as 0.5?, with a 0.1? resolution. However, there are three 
major obstacles to achieving this accuracy when the compass 1s mounted on a vehicle. 

First, the magnetic compass is referenced to magnetic north rather than true north. 
Therefore, the difference between true and magnetic north, or variation, must be 
compensated for prior to using the heading information for the filter. This variation is 
dependent on the location of the vehicle. While the average variation in an area can easily 
be obtained from the pertinent navigational chart, the actual compass reading can be 
affected by local concentrations of iron beneath the bottom of the sea. Since the vehicle is 


usually operating within 200 m of the ocean bottom, where local magnetic disturbances 





can be present, performance of the magnetic compass must be monitored to ensure that 
any magnetic disturbances are recognized. 

The second error is deviation, or the difference between a perfect magnetic 
compass and the compass in use. This is influenced by the construction of the compass as 
well as the presence of ferrous metals on the vehicle in the vicinity of the compass. 

The third source of error is more problematic. Whenever the vehicle's thrusters are 
used, the resulting magnetic field from the energized motors affects the output of the 
compass, resulting in output instability until the thruster-induced magnetic transients 
decay. For large-area mapping operations, the vehicle operates mostly on long, straight 
legs, so the effects of thruster-induced errors are minimized. Operations requiring more 
frequent maneuvering degrade compass accuracy accordingly. 

The characteristics of the magnetic compass are the opposite of the gyro: it does 
not drift with time but does exhibit significant short-term fluctuations. Because of its lack 
of drift, it can be used as a reference to reset the gyro when required. However, care must 
be taken to ensure that the magnetic compass is settled out when performing the reset. 
Most importantly, the vehicle thrusters should not be in operation during this procedure to 
avoid the subsequent error in compass heading output. As for the gyro, magnetic compass 


bias is included in the state space to provide a continuously updated estimate of its value. 


2.8 Inclinometer 


At DSL, the single-axis Watson inclinometer with angular rate sensor 1s used to 
provide pitch and roll information. This unit combines accuracy and reliability with small 
size and low power requirements, making it ideal for vehicle operations. 

The inclinometer uses a rate sensor and an integral vertical reference. À position 
output is obtained by integrating the angular rate output. By comparing this signal with the 
vertical reference, an error signal is generated. This error signal is filtered and sent back to 
the rate sensor as a bias. The system is also damped. With the overall system, the effects of 
inertia, damping, and short-term accelerations are reduced 


The angular rate sensor uses two piezoelectric bender elements, which are 


resonantly driven in opposite directions. Rotation causes a bending force, which 15 
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Fig. 2-7, Accelerometer sensor exploded view (Cady, 1984). 


demodulated to produce the rotation rate. The vertical reference uses a liquid-capacitive 
element. With these components, the system has no moving parts, which increases 
reliability. Overall, the system weighs less than eight ounces, making it easy to mount on a 
ROV (Watson, 1990). To obtain the necessary data, two of these units are used on the 


vehicle, mounted orthogonally. One provides pitch, the other roll. 


2.9 Systron Donner MotionPak (Inertial Motion Unit) 


The Systron Donner MotionPak provides the final sensor measurements required 
for the Kalman filter. It consists of two parts: the linear accelerometers and the angular 


velocimeters. 


2.9.1 Accelerometer Operation 


The linear accelerometer used for vehicle navigation in this application is the 
Sundstrand Data Control Q-Flex® accelerometer (see Fig. 2-7). Acceleration produces a 
torque on the sensor’s proof mass. A detector measures the displacement of the mass and 
produces a proportional output voltage. The resultant signal is amplified and fed to a 
torquer coil fixed to the proof mass. This current in the coil provides a restoring torque to 


balance the applied acceleration. The current also goes through a load resistor, generating 
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Fig. 2-8. Accelerometer system functional diagram (Cady, 1984). 


the output voltage for the detector (Cady, 1984). See Fig. 2-8 for an overall system 


schematic. 


2.9.2 Angular Velocity Measurements 


Angular measurements are provided within the IMU using the Systron Donner 
GyroChip™. This device uses a vibrating quartz tuning fork to sense angular rate by 
acting as a Coriolis sensor. The vibrating fork drives a similar pickup fork that produces 
the output signal. These two forks, along with their support flexures and frame, are made 
from a wafer of single-crystal piezoelectric quartz. The drive tines are driven by an 
oscillator that causes the tines to move toward and away from each other at high 


frequency. Each tine has a coriolis force acting on it given by 
Bean SV 


where 


m is the tine mass, 
Q is the input rate, 


V, is the instantaneous linear radial velocity. 


x 


The forces generated are perpendicular to the plane of the fork assembly at each of 
the tines and in opposite directions, yielding a torque proportional to the input angular 
velocity. The pickup tines respond to the oscillating torque by moving in and out of the 


plane, causing output signals to be produced by the pickup amplifier (Systron Donner, 





1993). A separate unit is provided for each axis, resulting in independent measurements 


for pitch rate, roll rate, and yaw rate. 
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Chapter 3 


Development Of The Kalman Filter 
Model 


3.1 Characteristics of the Linear Kalman Filter 


The purpose of the Kalman filter is to produce an unbiased, minimum variance, 
consistent estimate of a quantityx based on a set of measurements, z, where the 


relationship between these variables can be represented by 


"a 
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Therefore,z is a linear combination of the elements of the vector x, plus random noise 
represented asv (Gelb, 1974). The desired properties of such an estimate, as delineated 


above, are: 


Unbiased: the expected value of the estimate is the same as that of the actual quantity 
being estimated. 


Minimum Variance: the error distribution of the estimate is less than or equal to that of 
any other unbiased estimator. 


Consistent: as the number of measurements increases, the estimate converges to the 
true value of the quantity being estimated. 


If the noise is Gaussian and the relationship is linear, the Kalman filter has been proved to 
meet these criteria (Gelb, 1974). 

An additional characteristic of the Kalman Filter is that it is recursive, which 
means that new estimates can be computed without storing past measurements. Instead, all 
previous information is summarized in the current estimate and associated error 
covariance matrix. This recursiveness allows real-time processing of data without 


excessive memory storage requirements or computational burden. 
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3.1.1 System Model 


For a given real system to be represented using a Kalman filter, it must be put into 


a standard form. The two governing equations of the system model are: 
X =Ax+Gw+Llu 

and 
nun 


wherey 1s a deterministic control input,w is a random forcing function with error 
covariance O , andy is the measurement noise with error covariance matrix R.A,G,L,C, 
and D are matrices describing the relationship between the different vectors. 

The first equation states that the rate of change of the system parameters can be 
predicted using a linear combination of the present parameters and a linear combination of 
control inputs. The second states that the measurement vector, z, is a linear combination 


of the system parameters plus a linear combination of the present control inputs. Both 


equations include the effects of noise by virtue of thew andv terms. 


3.1.2 System Model Dynamics 


To produce an appropriate Kalman filter model, the state variables that make up 
the vectorsx andz must be defined. The vectory includes all parameters necessary to 
model vehicle behavior. The vectorz includes all measurements that can be obtained from 
the sensors as described in Chapter 2. Both must be determined using knowledge of the 
system dynamics and the available sensors. In this case, there are two different types of 
model propagation, one for vehicle motion and one for attitude. 

The primary control forces are those induced on the vehicle by the tow cable. 
However, these are virtually impossible to predict due to the realities of the towing 
operation. First, although the surface ship is proceeding predominantly on a known 
course, wave action produces continuous variations in pitch, roll, and heading. These 
variations are all propagated down the tow line to some degree, despite various techniques 


used to isolate vehicle motion from ship motion. Even if these forces on the tow cable at 
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Fig. 3-1. System model for X-position propagation. 


the surface could be measured precisely, the cable forces at the vehicle, several thousand 
meters away, are difficult to model. 

The vehicle model is also ill-defined. Changes in equipment configuration on the 
vehicle, which affect drag and added mass, can have significant effects on the vehicle 
dynamics. Also, when cable forces induce a change in vehicle attitude, the force of the 
water acting on the moving vehicle coupled with the designed self-righting characteristics 
of the vehicle combine to restore the vehicle to its mean velocity and attitude. Thus, the 
vehicle frequently follows a damped sinusoidal path in both vertical and horizontal 


directions. Therefore, the predominant characteristics of vehicle motion are: 


1. It is extremely unpredictable due to the inability to measure cable forces and to 
properly model vehicle dynamics. 


2. The vehicle tends to return to its mean attitude after a cable disturbance has sub- 
sided. 


As aresult, the vehicle accelerations are be considered to be influenced by a white- 
noise disturbance and control inputs are not modelled. To help compensate for any errors 
in these assumptions, linear accelerations are measured by the strapdown inertial motion 
unit (IMU) to provide current, though noisy, actual accelerations to the filter algorithm. 

For thruster-controlled vehicles, control input is often better defined. In these 
circumstances, operator-controlled inputs can be added as a deterministic vector 4, with 
an associated matrix L to mathematically relate the inputs to their effect on the state 
vector. 

To estimate vehicle position, the system model is as shown in Figure 3-1. X- 
position (latitude) is shown; Y-position (longitude) and Z-position (depth) follow the same 


A 


principles. 
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Fig. 3-2. System model for vehicle propagation. 


The system model for the angular positions is similar but requires only one 
integrator. Again, the system is modelled as disturbed by white noise. The angular 
velocity output of the IMU is used as the measurement for angular velocities (roll, pitch, 


and heading), shown in Figure 3-2. 


3.2 Sensor Outputs and Their Relationship to the 
State Space 

Before proceeding to the specifics of the state space model for this application, 
some details of the sensor data output must be described. To produce a useful Kalman 
filter for this application, it is essential to identify exactly what information ts available. 
Additionally, an expected frequency for receipt of data by the filter 1s necessary. 

During vehicle operations conducted by DSL, four different data packages are 
collected as the appropriate data are available. These four data streams must be translated 
into a format usable by the Kalman filter, and any potential gaps in receiving data must be 


anticipated and accounted for in the filter algorithm. 


3.2.1 IMU Package 


The IMU package consists of the linear accelerometers and angular velocimeters. 
As is the case for the other three data packages, the raw data stream is translated into a 
usable form for the filter using a relatively simple program written in C language. The 
IMU package provides data at a rate of approximately 10 Hz. The IMU'S outputs are all in 


sensor coordinates, which must be converted to body coordinates for use in the filter. 
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3.2.2 Attitude Package 


The attitude package combines the outputs of the depth detector, inclinometers, 
gyro, and compass to provide depth, roll, pitch, and heading. While individual sensors 
providing data operate at various frequencies, the overall package operates at 


approximately 5 Hz. 


3.2.3 Doppler Velocimeter Package 


Included in each data stream from the doppler velocimeter is considerable detail 
about the operator-selected parameters being used by the velocimeter. However, only the 
body-referenced velocities in each orthogonal direction and the velocity error 
measurement are used by the Kalman filter. Again, a C program ts used to translate the 
Hex-ASCH output into usable data, and the sensor-referenced outputs must be converted 
to body coordinates. 

As discussed in Chapter 2, the output frequency of the doppler velocimeter 
depends primarily on the number of pings per ensemble and the altitude of the vehicle 
above the bottom. For the sidescan-sonar mapping configuration employed during the 
DSL-120 deployment used for this thesis, the doppler velocimeter usually provided data 
approximately every 2-3 s. However, this time period was occasionally shortened or 


lengthened due to changing altitude and varying bottom conditions. 


3.2.4 Positioning System Package 


The positioning system has the most irregular of the four data streams received for 
vehicle navigation. Frequency of data reception depends on the type of positioning system 
in use. For systems using an acoustic transponder array, such as the long-baseline or short- 
baseline systems, this depends primarily on the distance from the vehicle to the acoustic 
transponder array. However, whenever data acquisition is secured (usually when a given 
track line is completed until the towing ship has maneuvered to get into position for the 
next track line), the positioning system is frequently secured as well, since precise 
knowledge of vehicle location is unnecessary except when obtaining sonar data. Of more 


concern is a loss of position information during sonar operations. There are several 
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different possibilities that can cause such a problem. A large bottom feature, such as a 
ridge, can physically block the path from the acoustic array to the vehicle. Another 
significant factor is the sound-velocity profile (SVP), which dictates the path sound waves 
will travel in the water. With an unfavorable SVP, acoustic position information can be 
lost only a short distance away from the transponder array. 

For systems not tied to an array, such as a terrain-relative positioning system, the 
frequency of obtaining information will be highly unpredictable since it depends on the 
available terrain and the quality of the database. It is likely that there will be frequent long 
gaps between fixes and that these gaps will exist whether or not sonar operations are in 
progress. 

This irregularity of position information is an important reason for developing a 
Kalman filter using the doppler velocimeter information. By filling in the gaps in position 
fixes with an accurate estimate of vehicle position, the sonar data quality can be greatly 
enhanced. 

Like the attitude data stream, the position data stream is typically easy to decipher. 
The components used for this Kalman filter are the X and Y positions (in meters from a 


reference point) and the time the datum was obtained. 


3.3 Defining the State Space 


Now that the system dynamics and available measurements are clearly identified, 
the state space can be defined. Once thex vector is labeled, the Kalman filter model can be 
mathematically developed. Using the coordinate system axes specified in Section 2.2, the 


components of the state vectory are defined as: 


X1: X-acceleration (All accelerations are in body, or vehicle-referenced, coordinates) 
X2: Y-acceleration 

X3: Z-acceleration 

X4: X-velocity (All velocities are in body, or vehicle-referenced, coordinates) 

X5: Y-velocity i 

X6: Z-velocity 
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X7: X-position (All positions are in earth-referenced coordinates) 
X9: Y-position 
X9: Z-position 


X10: Roll rate (Defined as rotation about the vehicle's Y-axis; positive roll is defined 
as port side up) 


X11: Pitch rate (Defined as rotation about the vehicle’s X-axis; positive pitch is 
defined as bow up) 


X12: Yaw rate (Defined as rotation about the vehicle’s Z-axis; heading is measured 
with normal compass orientation. To maintain the right-handedness of the system, this 
is converted within the Kalman filter algorithm to measure positive heading rate, 
usually called yaw rate, as counterclockwise, rather than the normal compass direction 
of clockwise) 


X13: Roll 

X14: Pitch 

X15: Heading 

X16: Gyro bias (Difference between gyro heading and true heading) 


X17: Magnetic compass bias (Ditference between compass heading and true heading. 
It includes both variation and deviation) 


Also, there is the measurement vector, z. When all four data streams are available to the 
filter, the components ofz correspond exactly to the components of the state vector x, 
with the addition of two heading measurements from the gyro and compass. As discussed 


later, however, this is seldom the case due to the variations in data stream frequency. 


3.4 Nonlinearity and the Extended Kalman Filter 


Unfortunately, the differences in coordinate systems between the various sensors 
means that the model cannot be treated as linear. With vehicle accelerations and velocities 
provided in body coordinates and positions provided in earth-referenced coordinates, a 
coordinate transformation must be made between the two references to allow the data to 
be used. This coordinate transformation is accomplished using standard trigonometric 


equations that use vehicle roll, pitch, and heading to relate body coordinates to earth 
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coordinates. This transformation is performed within the extended Kalman filter algorithm 


by altering the A matrix at each filter iteration as explained in Section 3.7.2.2 


3.4.1 Choosing the Extended Kalman Filter 


As stated in Section 3.1, the linear Kalman filter has been proved to provide an 
unbiased, minimum-variance, consistent estimate of the state vectorx as long as the noise 
is Gaussian. Noise considerations are addressed in Section 3.5. 

To compensate for the nonlinearity of the system, several techniques are available. 
Unfortunately, none of these can be proved to meet the same standards as the linear 
Kalman filter. In order of complexity, three possible solutions are the extended Kalman 
filter, the iterated extended Kalman filter, and the second-order filter (Gelb, 1974). 
Because it is the simplest and incurs the least computational burden, the extended Kalman 
filter is used to provide the state estimates for this application. As Gelb states, 

There is no guarantee that the actual estimate obtained will be close to the truly 
optimal estimate. Fortunately, the extended Kalman filter has been found to yield accurate 


estimates in a number of important practical applications (Gelb, 1974). 


3.4.2 Filter Propagation 


The basic method of producing a state estimate with the extended Kalman filter is 


the same as for the linear version. First, some definitions must be provided: 


፲ (4) 15 the estimate of the state vector using all data up to and including the present 
time. 


፳፪ (ያ + 110) is the predicted estimate of the state vector at the time of the next filter 
update step using all data up to and including the present time. 


x (r|[t — 1) is the predicted estimate of the state vector at the present time using data 
up to and including the time of the last filter step. It is the same as thex (1 + 1|) 
calculated after the previous filter update step. 


P (t|t) is the error covariance matrix of the state vector using all data up to and 
including the present time. 


P (t *- 1|r) isthe predicted covariance matrix of the state vector at the time of the next 
filter update step. 3 


P (t|t — 1) is the predicted esumate of the error covariance matrix at the present time 
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using data up to and including the time of the last filter step. It is the same as 
theP (t+ 1]t) calculated after the previous filter update step. 


For the linear Kalman filter, the error covariance matrices are deterministic and 
exact based on the assumptions used for input and measurement noise in the filter model. 
Therefore, they provide the current uncertainty of each variable in the state vector, 
assuming the model is correct. For the extended Kalman filter, the error covariance is only 
an approximation since the actual error covariance matrix cannot be calculated due to the 
nonlinearity of the system, a condition that can theoretically cause filter divergence. 
However, the extended Kalman filter has been found to work in numerous practical 
applications, with the only test being whether the filter works in actual use (Gelb, 1974). 
As demonstrated in Chapter 4, the good results using simulated and actual data support the 
assumption that the extended Kalman filter is an appropriate choice for this application. 

For filter propagation, current estimates ofx (r|r — 1) andP (r|r— 1) are available 
from the previous filter iteration. As explained in Section 3.1.1, the governing equations 


for the system model are: 
Y = Ax+ Gw 


and 


Il 


Cx+Du. 


tl y 


TheD y term is called the feed-forward term and represents the immediate effect of 


control inputs on output. As discussed earlier, control inputs are not modeled. 


3.4.2.1 Update Step 


The first step in the filter propagation is to determine the innovation term /, which 


is defined by the equation: 


^, 


l1 2 z—- (Cxx(rtr—-1)). 


This is the difference between the expected measurement vector obtained from the state 


estimatex (t+ 1|r) calculated during the previous filter update step and the actual 





measurement vector derived from the vehicle’s sensors. Next, the covariance matrix of 


innovation, v, is calculated: 
v = CxP(t|t-1) xC'«R, 


where& is the error covariance matrix of the measurement vector, z. The Kalman gain K 


is then calculated: 
K = P(tlt-1)xCxinv(v). 


The next step is to calculate the new state vector estimate using the value ofx (¢ + 1 |r) 


determined during the previous filter update step: 
x(qdo = 3 (፡፤/--1) ተጽ =/. 


Finally, the new error covariance is computed: 
P(t\t) = (Idene-KxC)xP(tt-1)x (Ident-KxC)'+KxRxK, 


where /dent in this case ıs the identity matrix with the same dimensions as K XC. 


3.4.2.2 Prediction Step 


For the standard Kalman filter, the predicted values of the state vector and the error 
covariance matrix for the next time step using data up to the present time are now 
calculated. Since the Kalman filter is implemented as a discrete instead of continuous 
filter, a conversion of the propagation equations from their continuous forms given in 
Section 3.1.1 to their discrete counterparts is performed. This transformation uses the 
MATLAB function "c2d", which takes the A and G matrices and the filter time step as 
inputs and provides the discrete matricesó (translation of the A matrix) andl” (translation 
of the G matrix) as outputs. Once the conversion to discrete matrices is made, the 


predictions are computed: 


፡‹(/+11|) = $4x xo) +1 xw, 
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1 ገሙ ን: ህ + TDR; 
where Q is the error covariance matrix for the white noise, w. 


3.4.3 Implications of the Extended Kalman Filter 


To obtain a new state estimate, the extended Kalman filter uses a first-order Taylor 
series expansion about the current state estimate to linearize the problem at each update 
step. In the steady-state version of a linear Kalman filter, the Kalman gainK is a constant. 


For the extended Kalman filter, it must be recomputed at each update step (Gelb, 1974). 


3.5 Modeling System Noise 

Prior to using the Kalman filter, predictions must be made concerning the 
estimated errors in the sensors used. All data used by the filter are weighted based on these 
predicted errors. A measurement considered very accurate has a strong effect on the state 
estimate. For example, if position information is considered very accurate relative to 
heading and velocity, the estimates of the latter two are altered from their measured values 
as necessary to ensure that the estimate of position is close to the measured value. Poor 
position information places more reliance on heading and velocity measurements. To 
ensure optimum filter performance, it is essential that the errors be modelled as accurately 
as possible. 

Another important assumption in the Kalman filter is that any errors in the control 
input vector and the measurement vector are unbiased and follow a Gaussian distribution. 
For nearly all cases, the assumption of Gaussian errors is valid considering the 
characteristics of the sensors employed. The prominent exception is the positioning 
system. While normally the errors can be treated as Gaussian, occasionally the system 
produces anomalously large errors in position. If these position fixes are treated by the 
filter as valid, improper changes in the elements of the state vector are made, resulting in 
rapid filter degradation. Identifying and ignoring these bad data is an integral part of the 


~ 


modified filter algorithm. 





3.5.1 Eliminating Non-Gaussian Position System Errors 


The grossly inaccurate position measurements occasionally produced by acoustic 
positioning systems are frequently referred to as "flyers". Removing them from the 
database 1s vital, but care must be taken. It the allowable tolerance between estimated 
position and measured position is too tight, valid position fixes can be rejected by the 
filter. Once this divergence between estimated and actual position occurs, the likelihood of 
ever accepting another position fix is greatly reduced. This problem is most acute when a 
long interval exists between fixes. It vehicle heading or velocity is estimated inaccurately 
during this interval, the difference between estimated and actual vehicle position grows. If 
the discrepancy becomes large enough, when position information is once again obtained 
it Is considered to be a flyer and rejected. With no further position information, the state 
estimate of position becomes increasingly inaccurate. 

To provide a window that is likely to reject flyers while accepting all valid fixes, 
the standard allowable error is established at ten times the standard deviation of the 
position system. To provide for time-dependent system errors, an addition is made to the 


allowable error based on the time since the last position system fix, given by 
Additional allowable error = 0.1m x At, 


where 


At = Time since last position fix in seconds. 


Fixes that yield a difference between the current estimated position and the position 
system fix of greater than this total allowable error are rejected. The method for 
determining the standard deviation is discussed in Section 3.5.3.2. 

The decision to use a cutoff point of ten times the standard deviation is not based 
on any formula. It is large enough to admit reasonable data, but still should reject any 
flyers. To help prevent filter divergence between estimated position and valid fix 
information, a count is maintained within the program of the number of rejected fixes. By 
monitoring this count during vehicle operations, filter divergence can be recognized by 


continuous fix rejection. The window of acceptance can then be adjusted as necessary to 
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allow the filter to accept new fixes and thus change the state estimates as appropriate to 


reflect the valid position information. 


3.5.2 Random Forcing Function Noise 


The random forcing function noise is expressed by the error covariance matrix ©. 
It is a 6x6 matrix since there are six terms that drive the system, three linear accelerations 
and three angular velocities. It is all zeros except on the main diagonal since each of these 
parameters is considered to be independent of the others, making the cross correlation 
terms Zero. 

The magnitude of the error covariance terms must be estimated from knowledge of 
the vehicle dynamics accounting for tow ship motion, tow line motion, and vehicle 
dynamics. For this filter, the standard deviation of the linear accelerations is set at 0.2 m/ 
s^, and that of the angular velocimeters at 2 °/s. These constants are incorporated into the 


KFSetup program. 


3.5.3 Measurement Uncertainty 


The measurement uncertainty is incorporated into the 17x17 error covariance 
matrix R. The portions of this matrix applicable to each sensor are addressed separately 


below. 


3.5.3.1 Linear Accelerometer Uncertainty 


The uncertainties in the linear accelerometer measurements have two sources. One 
is the noise and bias inherent in any instrument, which can be determined by test or by 
using the manufacturer's specifications. The other is caused by the displacement of the 
sensor trom the vehicle’s reference point for the body-reterenced coordinate system. 

As shown in Figure 3-3, the vehicle’s center of rotation and theretore its most 
convenient body-coordinate reference point is the point of attachment to the tow line. 
Assuming the vehicle is not experiencing an actual linear acceleration to the lett (-X 
direction), the X-accelerometer would sull sense acceleration if angular velocity in the 
yaw direction (change in heading) were non-zero. This phenomenon is governed by the 


following coupled equations (Catford, 1978): 
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Angular 
velocity Linear acceleration sensed due to angular velocity 


enter of rotation 
(tow point) 





IMU Sensor 


Fig. 3-3. Linear accelerometer output caused by angular velocity. 


Ay = uU+qw-vr+a,(-g2-r2) + ily a) mtd 
vow ሀው + av(r-rqp) UD r2) +a-(—p+qr) —gsindcosO 


0, =wrpv-quta,(-qt+rp) + (ly (p+rg) +a, (- p? - q2) — gcos Â coso 


where &,, Oy, anda, are the outputs of the accelerometers; dy, a, and a, are actual 
linear accelerations; ዘኔ v, and w are velocities; 6 2 roll; 92 pitch; p, g, rare the angular 
velocities; g 1s the gravily vector. 

To simplity the filter algorithm, this addition to linear acceleration measurements 
due to angular velocities 1s ignored. The justification is twofold: first, for a towed vehicle 
the magnitude of the linear acceleration caused by the angular velocities is small 
compared to actual linear accelerations; second, over a relatively small time interval, the 
angular velocities have a mean of zero due to the damped sinusoidal motion of the vehicle. 
Therefore, any errors induced by this simplification are cancelied out when the angular 
velocity reverses direction. As a result, the standard deviation for the linear acceleration 


ጋ 
measurements are set at 0.1 m/s*. 


3.5.3.2 Doppler Velocimeter Uncertainty 

As part of its data stream, the doppler velocimeter provides an error velocity. This 
error velocity is calculated by the velocimeter using a proprietary RDI Instruments 
program. Factored into this calculation are the depth cell size, the altitude of the vehicle 


above the bottom, the vehicle velocity, and the number of pings per ensemble (RDI, 1993). 
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This error velocity is used by the Kalman filter program as the standard deviation for 
velocity measurements. Therefore, these elements of the A. matrix vary with each filter 


update step. 


3.5.3.3 Position System Uncertainty 

Ihe standard deviation for acoustic positioning systems is determined based on 
operator knowledge of the system and takes into account such parameters as system 
frequency and distance from the transponder array. It is established prior to filter operation 
as part of the KFSetup program but can he changed during operations if desired. For other 
systems, such as terrain-relative position fixes, the standard deviation can be made part of 
the data stream based on the estimated accuracy of the fix and can therefore be varied from 
Ax to fx. 

Of all the measurement uncertainties, this requires the greatest attention. [f£ it 1s 
assigned to be too large, the Kalman filter does not weight position fixes enough, resulting 
in potentially significant errors in the estimated position. This can adversely affect the 
quality of any sidescan-sonar or video data obtained during vehicle operations. 
Conversely, if it is set too small there is an increased danger of creating sufficient 
divergence between the estimated and actual positions to cause invalid position fix 


rejections. 


3.5.3.4 Depth Sensor Uncertainty 

When operating in deep water, the depth sensor has one important drawback: 
because of the limitations of its computer bit capacity, it can only resolve depth to the 
nearest 0. 1m. Coupled with the normal fluctuations of the instrument, this tends to make 
the output vary frequently within a 0.4-m band. Therefore, the standard deviation for 
deep-water operations is set at 0.1m, which places nearly all fluctuations within two 
standard deviations of the actual value. This standard deviation can be reduced 


appropriately for shallow-water operations when the resolution is higher. 
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3.5.3.5 Angular Velocimeter Uncertainty 
Estimation of the angular velocimeter uncertainty is fairly straightforward. Prior 
testing or manufacturer's specifications are used to determine the expected standard 


deviation. For the instruments in use, this is set at 0.5°/s. 


3.5.3.6 Pitch and Roll Uncertainty 

Like the depth sensor, the pitch and roll sensors are digital and therefore have a 
noticeable minimum resolution. For these sensors this resolution is 0.19, and normal 
fluctuations in a 0.2? band are observed. To account for unmeasurable biases and to 
prevent the filter from weighting these measurements too heavily, a standard deviation of 


0.5? is assigned. 


3.5.3.7 Heading Uncertainty 
Heading is measured directly by the magnetic compass and the gyro. For each, the 


applicable formula is 
True Heading = Indicated Heading - Sensor Bias. 


Since the bias for each sensor is modeled within the Kalman filter as a state variable, 
additional measurements improve the estimate of bias and thereby make the estimate of 
true heading more accurate. 

The standard deviation of the magnetic compass is constant and is set at 0.5? to 
account for normal fluctuations. If frequent thruster operations are envisioned, this 
standard deviation may have to be increased due to the effects of the magnetic fields 
induced during thruster operations. Due to its greater stability, the gyro has a constant 
standard deviation set at 0.1°. 

To provide an independent measurement of true heading, which is necessary to 
correct sensor biases, two different techniques are possible. For a highly maneuverable 
vehicle operating in a small area, an acoustic transponder can be mounted both at the front 
and at the back of the vehicle. By comparing the positions of each transponder, true 
heading can be derived. The standard deviation for this technique depends on the accuracy 


of the positioning system used. 
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Calculated heading 
based on least- 


squares fit 


Fig. 3-4. Calculation of true heading and standard deviation. 


For a towed vehicle, vehicle motion can be assumed to be nearly entirely in the 
direction of the vehicle’s front. Therefore, a measurement of true heading is obtained by 
calculating a least-squares fit to the last four position fixes. Although the standard 
deviation of this heading measurement is difficult to calculate precisely, an approximation 
is derived as follows (see Fig. 3-4). 

After obtaining the least-squares solution, a separate heading measurement 1s 
Mae eW een cach adjacent pair of fixes (and 2, 2 and 3, 3 and 4). The ditterence 
between each of these heading measurements and the overall least-squares measurement 
is averaged. This average difference in heading measurements is considered to be the 
standard deviation of the measurement. Although this is not precise, it has the desired 
property of providing less standard deviation as the fix measurements become less 


scattered from the best-fit track. 


3.6 Establishing Reference Points and Initial Conditions 


3.6.1 Reference Points 


To ensure correct matrix manipulations while avoiding round-off errors, MATLAB 
works better with well-conditioned matrices, which means that the elements of the 
matrices are as close together as possible in terms of order of magnitude. To achieve this 
conditioning, reference points for position and time must be established. These reference 


values are subtracted from the actual values prior to use in the filter. The filter uses only 
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the difference between the reference values and the current values, resulting in numbers 
smaller by several orders of magnitude. 

For X- and Y-positions, the Universal Transverse Mercator (UTM) system is used. 
This system divides the oceans into rectangular grids with scales in meters. A suitable 
reference point is chosen in the area of vehicle operations and included in the KFSetup 
program. 

The Z-position reference point is dependent on expected vehicle operating depth. 
To achieve a right-handed coordinate system, the filter actually uses vehicle altitude above 
or below the reference point rather than depth, but this coordinate system change is 
imbedded within the filter algorithm. A reference depth near the expected operating depth 
is chosen by the operator in the KFSetup program prior to commencing operations. 

The final reference point is time. Each sensor package data stream contains a time 
stamp with units of Julian days. The reference time for each day is the beginning of the 


Julian day. Therefore, the time value used in the filter is always a fraction of a day. 


3.6.2 Initial State Vector and Error Covariance Matrix 


Prior to the first filter update step, the initial state vectorx and error covariance 
matrix P must be provided. Normally, the initial sensor data obtained are used tor the state 
vector, and the predetermined R matrix is augmented with the initial calculated variable 
elements of R (for velocimeter and heading measurements) to provide P. However, if 
some other more accurate method is available for determining any of these variables, the 


better value can be substituted instead. 


3.6.3 Removing Known Biases 

Of crucial importance in optimizing Kalman filter performance is the removal of 
bias. Uncorrected bias provides a consistent source of error in the state estimate vector and 
must be eliminated as much as possible. 

Because of its use of doppler to measure velocity, whereby zero frequency change 
equates to zero velocity in that direction, the doppler velocimeter is relatively immune to 


a 


bias. Therefore, no attempt is made to compensate for it. 
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The position information is also insensitive to bias, since the use of a reference 
point automatically removes bias from the system. Once again, no bias correction 1s 
needed. 

The bias of the pitch and roll sensors is directly related to the precision of their 
placement on the vehicle. Normally, the final installation of the sensors on the vehicle is 
performed in the lab, which allows the sensors to be checked to ensure they are unbiased. 
Therefore, no bias adjustment is made in the filter. If the sensors must be replaced at sea, 
accurate measurement of pitch and roll on a moving ship is virtually impossible, so there 
is nO way to measure bias accurately. In this case, the sensor is mounted as precisely as 
possible and the filter assumes unbiased measurements. Since pitch and roll are usually 
rather small, the effects of undetermined bias on the coordinate transformations is 
negligible. 

Compensation for gyro bias and magnetic compass bias is discussed in Section 
3.5.3.5. Additionally, any known initial bias is included as the initial condition of the state 
variable for each heading sensor bias. Short-term effects on magnetic compass bias caused 
by thruster operation cannot be calculated and are therefore ignored. 

As discussed in Section 3.5.2, the biases inherent to the linear accelerometers and 
angular velocimeters must be determined by laboratory testing. With the vehicle 
motionless and with zero pitch and roll, the output voltage of cach instrument can be 
measured. The voltage caused by the gravity vector 1s calculated so as not to be removed 
from the instrument measuring vertical acceleration, but the other measured voltages are 


subtracted from the data. These biases are also included in the KFSetup program. 


3.7 Changes Required to the Standard Kalman Filter 

In this application, there are three major changes that must be made to the standard 
Kalman filter described in Section 3.4. First, due to the variability of the data update rate, 
à constant filter update frequency ts impractical. Second, the difference in coordinate 
systems among the various sensors must be addressed. Finally, there are gaps in certain 
data streams, especially for X- and Y-position fixes. The filter must be able to function 


despite these gaps to provide continuous state estimates. 





3.7.1 Adjustments Required Due to Varying Filter Update Rate 


3.7.1.1 Determining the Desired Time Step 

To determine the desired time step, competing objectives must be considered. In 
support of a longer time step, there are two major arguments. First, adequate time must he 
given to obtain the data necessary for the propagation of the filter. Second, the interval 
must be long enough to permit real-time processing of the data. 

There is only one counterpoint in support of a shorter time step, but it is a vital one. 
It the time step 1s too long, the true vehicle dynamics will be lost, especially if significant 
maneuvering is occurring. Although a long time step may provide a reasonable 
approximation of average vehicle position and attitude, the lack of precise, continuous 
information will degrade any sidescan-sonar or video imaging accuracy. A compromise is 
necessary. 

Of the four data packages described in Section 3.2, only two have update rates that 
are both consistent and rapid. The IMU package produces data at approximately 10 Hz, 
while the attitude package operates at about 5 Hz. Therefore, the Kalman filter is designed 
with a minimum requirement of having data from these two packages prior to performing 
a filter update step. As an overall compromise, a ume Step of 0.5 s is used. This is short 
enough to capture the system dynamics, but long enough to allow real-time processing of 
data and to ensure both IMU and attitude package information is normally available. 

To prevent filter processing error in the event of an interruption in data flow, the 
filter algorithm checks to ensure that at least one sample of cach data package is available 
within the 0.5 s of data to be processed. If either is missing, the filter continues to accept 
new data until both are present. As soon as this condition is met, the data obtained from 
each sensor during that time step are averaged and the next filter update step is pertormed. 


3.7.1.2 Predicting the Next State Vector and Error Covariance Matrix 


ጊ 


In the standard Kalman filter, the last step of filter propagation produces the 
predicted values ofx (+ + 1]1) and P (t * I|r) . As described tn Section 3.4.2.2, one of the 
inputs required for the conversion from a continuous to a discrete matrix is the magnitude 
of the time step. For this filter. that magnitude is unknown. First, it will probably not be 


exactly 0.5 s, since the actual time step is the difference in time between the first and the 
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last data received prior to 0.5 s elapsing. Second, if cither IMU package or attitude 
package data ıs not received during that first 0.5-s interval, the filter algorithm waits until 
the missing data are provided. Thus, the time interval could be considerably longer. 

To correct for these varying time steps, a small change in filter mechanics is 
required. Instead of computing the predicted values at the end of a given filter update step, 
the filter algorithm waits until the beginning of the next filter update cycle. Therefore, the 
filter time step 1s known since the algonthm has already allowed 0.5 s to clapse and has 
then checked for the two data streams, waiting as necessary to obtain them. By allowing 
the predicted values to lag until the next time Step is confirmed, the continuous-to-discrete 


conversion can be made accurately. 


3.7.2 Coordinate System Transformations 


Due to the difference in coordinate systems among the various sensors, two 
separate calculations are required. Both use the current estimates of roll, pitch, and 
heading as input. The first removes the effect of the gravity vector on the linear 
accelerometer measurements. The second computes the currentA matrix used in the 


prediction of the next state estimate and error covariance matrix. 


3.7.2.1 Removing the Effect of the Gravity Vector 

To provide an accurate reflection of vehicle linear acceleration, the output of the 
IMU package must be modified to remove the gravity vector bias. This vectorg is 
assumed constant with a value of 9.8 m/s? pointing in the -z direction in carth-referenced 
coordinates. The filter algorithm uses the current state estimates of roll and pitch to 
calculate the component of the gravity vector included in the output of each of the three 
linear accelerometers. The effect of the gravity vector on x-acceleration is shown in Fig. 
3-5 as an example. The calculations are as follows (ቁ. = roll,@ = pitch, g = —9.8): 


` 


Actual x-acceleration = (Measured x-acceleration) + gsing, 


Actual y-acceleration = (Measured y-acceleration) — gsing, 


Actual z-acceleration = (Measured z-acceleration) — gc0s0cos®. 
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Fig. 3-5. Effect of gravity vector on measured x-acceleration. 


These calculations are actually approximations because the effect of one attitude 
on the measurement of another is ignored. For example, as pitch increases the sensitivity 
of the roll sensor is decreased due to the lessened gravity vector component on the 
inclinometer's sensitive axis. In the worst case, a pitch of 90 degrees results in no 
sensitivity to roll. For typical vehicle operations, during which pitch and roll are normally 


less than 10 degrees, the errors introduced by these approximations are negligible. 


3.7.2.2 Calculating the A Matrix 

As explained in Section 3.4.2.2, part of the prediction step of the filter update cycle 
converts the continuous time A and G matrices into their discrete time counterparts. 
Unlike the linear filter case, in this nonlinear filter the A matrix must be recalculated 
during each prediction step. 

To be more specific, the detailed version of the system propagation equation is as 
follows. The A matrix 1s constant with the exception of the 3x3 portion identified with 
“a ,” because this portion of the A matrix propagates the change in vehicle position due to 
the previously estimated vehicle velocities. Since vehicle velocities are body-referenced 
and positions are earth-referenced, a coordinate transformation is necessary. 

The current heading estimate, which is maintained in the traditional clockwise- 
positive system used by the compass and gyro, is altered to à counterclockwise-positive 
yaw angle to maintain a right-handed coordinate system. With units of radians, this yaw 


angle 1s calculated by: 
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K = 2r - (Current Heading Estimate). 


The 3x3 nonzero matrix within the A matrix is computed as follows. Required inputs are 


the current state estimates of roll (6), pitch (0), and heading (K ). 


Element (1,1) = cos cosx 

Element (1,2) = sin@singdcos kK — cos@sink 
Element (1,3) = cos@singcosk + sin@sink 
Element (2.1) 2 sinkcoso 

Element (2,2) = sin@singsink + COSMCOSK 
Element (2,3) = cos@singsink — sin@cosk 
Element (3,1) = -sınd 

Element (3,2) = sin@cosd 


Element (3,3) = coswcosd 
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3.7.3 Varying the C Matrix with the Available Data 


Section 3.7.1.1 describes the process by which the filter ensures it has both IMU 
and attitude information available prior to performing an update step. During this period 
of accepting new data, doppler velocimeter and/or X-Y position information may also be 
obtained. To perform the update step correctly in light of the available data, the filter 
algorithm keeps track of which data are available prior to performing the update operation. 
After averaging all data from each sensor, the C matrix must be computed for use by the 
filter. As part of this computation, the measurement vectorz 1s also determined based on 
which sets of data have been obtained. 

After the first four position fixes have been obtained (and therefore true heading 
information is available with cach subsequent fix), there are four possibilities for the C 
matrix and measurement vector: 


1. Only IMU package and attitude package data are available. The measurement vec- 
tor iS: 


x-acceleration 

y-acceleration 

z-acceleration 
z-position 


roll rate 


ሆ-1 


2 pitch rate 
yaw rate 
roll 
pitch 
heading by gyro 


heading hy compass 


and the corresponding C matrix is: 


A 





IONES OO DADA O 
0100000000000000 0 
0010000000000000 0 
0000000010000000 0 
0000000001000000 0 
E= OO O OOO 
0000000000010000 0 
0000000000001000 0 
ከ111.) 000505050 APODO 
000000000000001-1 0 
000000000000001 0 -I 


reflecting the position in the state estimate vectorx of x-acceleration (x1). y-accel- 
eration (x2), z-acceleration (x3), z-position (x9), roll rate (x10), pitch rate (x11), 
yaw rate (x12), roll (x13), pitch (x14), heading (x15), gyro bias (x16), and com- 
pass bias (x17). 

Both attitude package and XY position information are available but no doppler 
velocimeter data. The measurement vector 1s: 


x-acceleration 

y-acceleration 

z-acceleration 
X-position 
y-position 
Z-position 


roll rate 


ed 


pitch rate 
yaw rate 
roll 
pitch 
heading by gyro 
heading by compass 
true heading | 


and the corresponding C matrix is: 


58 





FOO ORONO O O00000 00 
0100000000000000 0 
DIVE ZUENDER) 
ከየ ከ ደ ከ" ኔሌ»ሌጅ፡ስ በ ፕህፕን ስ ስ 161) 
ሸህ ከ ከ) ከ ከው 1 ከከ ከ EO OR set) 
CFU ECO COO ORO eter EN 
C 0000000001000000 0 
0000000000100000 0 
0000000000010000 0 
0000000000001000 0 
ከታ ከህ ከ ስበ ከ11! በ1] ስውን 30) 
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3. Both attitude package and doppler velocimeter data are available but no XY posi- 
tion information. The measurement vector 1s: 


x-acceleration 

y-acceleration 

z-acceleration 
x-velocity 
y-velocity 
z-velocity 


Z-position 


Pd 


roll rate 
pitch rate 
yaw rate 
roll 
pitch 
heading by gyro 


heading by compass 


and the corresponding C matrix 1s: 


nU 





1000000000000000 0 
0100000000000000 0 
0010000000000000 0 
0001000000000000 0 
0000100000000000 0 
0000010000000000 0 
Cs 0000000010000000 0 
0000000001000000 0 
0000000000100000 0 
0000000000010000 0 
0000000000001000 0 
0000000000000100 0 
0000000000000 01-1 0 
lo 00000000000001 0 E 


4. The final case is when data are available from all sensors. The measurement vector 
uses the same construction as the state vector. 


x-acceleration 

y-accelerauon 

z-acceleration 
x-velocity 
y-velocity 
z-velocily 
x-posttion 
y-position 


= Z-position 


ሆ-) 


roll rate 
pitch rate 
yaw rate 
roll 
pitch 
heading by gyro 
heading by compass 


true heading 


while the C matrix is: 
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By using this varying C matrix, the Kalman filter mechanics remain the same for 
each update step regardless of the available data. Additionally, the error covariance matrix 
automatically reflects the data available. For each filter update step, each state variable's 
error covariance is increased due to elapsed time, and then decreased if a measurement of 
that state variable is available. If a given measurement Is not available, the corresponding 
State estimate has a greater error covariance than before. Similarly, available data reduces 
the error covariance appropriately based on the assigned standard deviation of that 


particular sensor output. 


3.7.4 Incorporating Fading Memory 


One problem with the standard Kalman implementation is filter “laziness”. Ina 
completely observable system, the Kalman gain approaches zero as time goes to infinity. 
Therefore, the filter ignores new data since it believes that it has essentially perfect 
knowledge of all state variables. Changes in those variables caused by vehicle operations, 


ው 


such as a change in course or velocity, are not reflected in the filter. 
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Gelb provides a simple solution to this problem using recursive fading memory 
filtering. To force the filter to continue using new data, the error covariance of the 
predicted state vector 1s artificially increased (Gelb, 1974). This is accomplished with a 


simple addition to the algorithm for calculating P (t+ 1]t) . Now, the equation is: 
P(r*l|t) =s x0xP (dro) xo -PxQxI', 


with the only change being the addition of the s term, which is the fade factor. This is 


calculated by 
s = exp (Afr/T) , 


where 

At 1s the measurement interval (0.5 s), 

t 1s the age-weighting time constant. 
For this filter, the time constant is chosen to be 30 s to ensure a filter responsive to rapid 
changes in vehicle parameters while still retaining sufficient memory. The resulting fade 
factor is 1.0168. 

As Sorenson and Sacks explain (Sorenson and Sacks, 1971), the use of this fade 
factor does not affect the stability of the Kalman filter. The only disadvantage is that by 
using the fade factor, the error covariance matrix is altered. Therefore, the error 
covariance matrix is no longer the best estimate of the error in the state estimate but only 


an approximation. 


3.8 Observability 

If a system is completely observable, the state vector can he entirely determined 
from the measurements. If the system is only partially observable, then any system errors 
involving unobservable quantities that propagate through the filter cannot be identified by 
measurements. For example, if position is not observed, then any consistent errors in 
velocity measurements will continue to increase the position error. In contrast, if velocity 


is not measured but position is, velocity can still be observed due to its known effect on 





position. In the context of vehicle navigation, a partially observable system means that 
there is no way to provide a reasonable estimate of the unobservable quantities. If that is 
the case, the filter will be useless. 

To be observable, the matrix (C EC MAT yc) must have rank n 
(Gelb, 1974). When all data are available, the C7 matrix has 17 linearly independent 
columns by inspection, and the system is trivially observable. However, when fewer data 
are available the observability changes. When only IMU package, attitude package, and 
doppler velocimeter information is available, the observability matrix 1s no longer trivial. 
Powers of A? greater than two yield only zero matrices and do not contribute to 


observability. Eliminating zero columns and repeated columns, 


100000000000 90 0 
010000000000 0 0 
001000000000 0 0 
0001000000000 0 
VODOCTODOO OOD 6 0 
0000010000000 0 
0000000000000 0 
0000000000000 0 

[eu ue ጅሚ 000000 O 
000000010000 0 0 
0000000010000 0 
0 ከ 908) 
0000000000100 0 
OOO 
0000000000001 I 
000000000000-1 0 
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The Greek lettersa, B, T, y, A, and 6 represent elements of the 3x3 coordinate 
transformation matrix. In this case, they do not add to the rank of the matrix since column 
15 containing them is simply a linear combination of columns four through six, while 
column 16 is a linear combination of the first three columns. Therefore, the rank of the 


matrix is only 14, leaving three state variables as unobservable. Not surprisingly, two of 
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the unobservable states 1n this case are X- and Y-positions. Without observing position, 
there is no way to determine if the estimates of velocity and attitude contain errors causing 
divergence of the state estimates from the actual values. The third unobservable state 
concerns gyro and compass bias. Without position measurements, true heading 
measurements are impossible. While independent measurements of gyro and magnetic 
compass headings allow the filter to observe the difference between the two hiases, 
without true heading measurements the actual value of either cannot be determined (since 
the difference can be calculated, knowing one of these biases would permit calculation of 
the other. Thus there is only one additional unobservable state due to the lack of true 
heading information). 

Similar calculations can be performed for the other two possible data 
combinations. Lacking only velocity data, the system 1s observable since position 
measurements can verify the accuracy of velocity estimates. Without X- and Y-position 
information available at least occasionally, the filter cannot be expected to provide valid 


estimates. 
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Chapter 4 


Kalman Filter Performance Using 
Simulated Data 


4.1 Purpose of Simulation 


The initial tests of this Kalman filter are performed using simulated data. This 
allows simple modification of data parameters to evaluate filter performance under a 
variety of conditions. Several tests are performed using the same basic data set. The 


specific parameters for each test are explained in detail. 


4.2 Data Generation 


4.2.1 Method of Data Generation 


The total time of the simulation is fixed at one hour. This provides ample time for 
filter evaluation and coincides with the period used for the actual data evaluation 
conducted in Chapter 5. 

Generating a valid data set is a multistep process. First, the control inputs are 
determined using reasonable values for linear accelerations and angular velocities. Then 
the nominal standard deviations for the control input measurements and system 
measurements are computed, along with system initial conditions. These use the values 
determined in Section 3.5, unless otherwise noted. For all simulated data, initial 
conditions are found in Table 4-1. 

To generate noise-free measurements in the appropriate coordinate system for each 
sensor, first the attitude measurements are computed for the entire hour using the 
MATLAB function "dlsim," with the angular velocity measurements as input. Using these 
nolse-free attitude computations, the vehicle-referenced linear accelerations are 
transformed into earth-referenced coordinates using the coordinate transformation found 


in Section 3.7.2.2. Next, "dlsim" is used again to provide earth-reterenced velocities and 





Table 4-1: Initial Conditions for Simulations 












Parameter Initial Value 


rien | 8. - 
rin | 9 
ጨመ: | o 
rs ፡. 


positions. Finally, attitude is used to transform the computed velocities into the vehicle- 






referenced coordinates used in the filter. 

After these noise-free measurements are computed, noise 1s added using the 
MATLAB function “randn,” which provides a Gaussian distribution with the desired 
variance. Additionally, the frequency of data output from each sensor is determined. The 


last step is to add desired gyro bias, compass bias, and any gyro drift. 


4.2.2 Noise-free Data Used for All Simulation Tests 


To provide a convenient basis for comparison among the tests, the same noise-free 
data set is used for each. Figures 4-1 through 4-4 provide a graphic display of a sample of 


these measurements. 
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Fig. 4-1. Noise-free measurement of Y-velocity. 
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Fig. 4.2. Noise-free measurement of Y-position. 
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Fig. 4-3. Noise-free measurement of vehicle pitch. 
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Fig. 4-4. Noise-free measurement of vehicle heading by gyro. 
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Fig. 4-5. Comparison of measured and estimated X-Y position. 


4.3 Simulation Tests 


4.3.1 Test 1: Accurate Initial Conditions 


For the first test, doppler veloctmeter readings are provided every 10 s and X-Y 
position information every 100 s. The initial conditions provided for the state variables 
match their actual values, including gyro and magnetic compass biases. Additionally, a 
gyro drift of 5°/hr is simulated. 

Figure 4-5 shows a comparison of actual to estimated position. Position data are 
available frequently, allowing the filter to maintain a close match between estimated and 
actual positions. Figure 4-6 provides a close-up of a smaller part of the data to allow better 
resolution. 

Estimating parameters with frequent data updates 1s relatively easy for the filter to 
accomplish. For example, Fig. 4-7 shows a portion of the data set comparing actual and 
estimated Z-position; Fig. 4-8 shows the same for roll. Heading is a more difficult state to 
estimate due to the inherent biases of the available instruments, the gyro and the compass. 


Figure 4-9 shows the estimate of heading throughout the data run; Fig. 4-10 shows a 
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Fig. 4-6. Close-up of X-Y Position Comparison. 
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Fig. 4-7. Comparison of actual and estimated Z-position. 
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Fig. 4-8. Comparison of actual and estimated roll. 
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Fig. 4-9. Comparison of actual and estimated heading. 
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Fig. 4-10. Actual and estimated gyro bias. 


comparison of actual and estimated biases for the gyro. With the trequent availability of 
data, the filter provides a good estimate of the heading and associated biases. Overall, with 
Gaussian noise and frequent data the filter performs well. Subsequent tests check its 


performance under more strenuous conditions. 


4.3.2 Test 2: Inaccurate Initial Conditions Combined with Position Data 
Gap 


For the second test, all parameters are the same except for those noted in Table 4-2. 
Additionally, no position information is available for a 500-s interval towards the start of 
the run.. 


Table 4.2: Initial Conditions for Simulation Test 2. 
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Fig. 4-11. Comparison of measured and estimated X-Y positions. 


As can be seen in Fig. 4-11, the lack of early position error combined with the 
incorrect initial heading and biases causes some inaccuracies during the ume without 
position data. However, after position information 1s again received the filter quickly 
adjusts to correct itself. 

The large error in the initial heading estimate is somewhat artificial since in actual 
operation, the towed vehicle should normally be within a few degrees of the surface ship’s 
average heading. However, even with a large initial error heading, gyro bias, and compass 
bias all approach their actual values by the end of the hour, as seen in Figs. 4-12 through 
4-14. If position information had been available throughout the data run, the final 


estimates would have been even closer. 


4.3.3 Test 3: Estimation Without Doppler Velocimeter 


The last test uses exactly the same parameters as test 2, except that no doppler 
velocimeter information is included. Under these conditions, un estimate of the .. 


importance of the velocimeter can be made. The same graphs are produced for 
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Fig. 4-12. Heading estimate. 
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Fig. 4-13. Estimated and actual gyro bias. 
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Fig. 4-14. Estimate of magnetic compass bias. 


comparison. As can be seen in Figure 4-15, the gap in position data combined with the 
lack of accurate velocimeter information creates such a large error in position due to 
velocity errors that all subsequent fixes are rejected by the filter algorithm. This renders 
position unobservable, and filter divergence results. However, since the compass and gyro 
are still producing independent sources of heading information, the filter is able to make 
some corrections to bias. Without fixes to provide true heading measurements, the heading 
does not converge to its actual value but uses a weighted average of the gyro and compass 


measurements to minimize the error. 
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Fig. 4-15. Comparison of measured and estimated X-Y position. 
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Fig. 4-16. Estimated heading. 
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Fig. 4-17. Estimated magnetic compass bias. 
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Fig. 4-18. Comparison of actual and estimated gyro bias. 
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Chapter 5 


Kalman Filter Performance Using Actual 
Data 


5.1 Description of Data 

The data used for this evaluation were obtained in July, 1994, during operation of 
the DSL-120, a towed vehicle mounting a sidescan sonar. Both data sets used for filter 
testing comprise approximately one hour of navigational sensor information. Due to the 
specific characteristics of the data package, two modifications were made to the Kalman 
filter algorithm to allow proper processing. 

First, gyro data were not obtained. Therefore, only the magnetic compass is used 
as the heading sensor for this data. As a consequence, the order of the filter is reduced by 
one due to the removal of gyro bias as a state variable. 

Second, analysis of the data reveals some anomalies. During the first data run, 
numerous spurious velocities of approximately 30 m/s are observed. In the second data 
Jo 


run, regular spurious compass readings of exactly 40° are found. These anomalies are 


removed before using the data in the filter. 


5.2 First Data Set 

On initial inspection, the first data set appears to be excellent for Kalman filter use 
due to the frequency of obtained data, especially from the long-baseline positioning 
system. However, closer analysis reveals that the behavior of the linear accelerometers 1s 
erratic. Figure 5-1 shows an example of this behavior. The difference between the two 
concentrations of outputs 1s 0.08 g, which corresponds to a linear acceleration of 
approximately 0.8 m/s”. If the filter uses this erratic accelerometer data, it rapidly diverges 
due to the large estimated accelerations translating into large, nonexistent velocities. 


Therefore, to run the filter on this data set the accelerometer measurements are set to zero 
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Fig. 5.1. Output of Linear Accelerometer. 


tor the entire run. The Kalman filter uses the remaining measurements of velocity, 
position, roll, pitch, and heading to estimate all parameters. 

Figures 5-2 and 5-3 show the difference in measured and estimated X-Y positions. 
The doppler velocimeter provides accurate measurements of velocities, which 
compensates for the lack of accelerometer measurements. Therefore, the filter is able to 
track position well. 

As an example of a parameter which has the advantage of continuous 
measurements, a comparison of measured and estimated roll 15 shown in Fig. 5-4. The 
discrete character of the sensor measurements compared to the continuous Kalman filter 
estimate 1s obvious. 

The last estimate meriting special attention is heading. With the frequent fixes, true 
heading measurements are readily available. As can be seen from Figs. 5-5 and 5-6, 
occasionally an maccurate true heading measurement will temporarily affect the estimated 


heading and bias. 
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Fig. 5.2. Comparison of estimated and actual X- Y positions. 
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Fig. 5.3. Close-up of X-Y position comparison. 
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Fig. 5.4. Comparison of measured and estimated roll. 
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Fig. 5.5. Estimated heading. 
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Fig. 5.6. Estimated compass bias. 


5.3 Second Data Set 

The second data set poses more of a challenge for the filter, since after four initial 
fixes are obtained there is a gap of nearly one-half hour without further X Y-position 
information. However, the linear accelerometers do not show the same erratic behavior 
during this data set and are therefore used as part of the measurement vector. Figures 5-7 
through 5-12 show the performance of the filter in estimating position in light of this data 
gap. When position information ts restored, there is a discontinuity in the filtered estimate 
of position due to the error developed by the filter estimate during the gap. After frequent 
fix information is again received, filter position estimates track with actual positions 
obtained from the long-baseline navigation system. 

Besides position, the most important vehicle parameter to estimate accurately is 
heading. Without an accurate heading, sidescan sonar information will be inaccurate as 


well. Since both the gyro and the compass can be biased, a vital characteristic of the filter 
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Fig. 5.7. XY-position estimates (:) compared to position fixes (0). Note the presence of a 
flyer in the upper left corner, which was properly rejected by the filter. 


is its ability to estimate heading. Figure 5-112 shows the estimated heading during the 
hour. To understand filter behavior in the vicinity of this discontinuity, X-position 
behavior is examined individually in Fig. 5-10. After a fix 1s received following the long 
data gap, the first fix is weighted heavily and the estimated X-position jumps to the 
location of the fix. As can be seen in the figure, however, that first fix was rather 
inaccurate, although still within the specification of the fix rejection filter. Subsequent 
fixes are weighted less heavily since the error covariance matrix is adjusted after receipt of 
the first fix. Therefore, estimated position adjusts to actual position over several fixes. 

The jump in estimated heading at t=2000 seconds is a result of the gap in position 
information. However, despite this lengthy gap, the estimated heading remained 
consistent within three degrees. 

Parameters with more frequent data update rates are easier for the filter to estimate 
since measurements are continuously matched against the estimates. As one example, the 
comparison of estimated Z-position for a small portion of the hour is shown in Figure 5- 
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Fig. 5.8. Comparison of estimated and actual vehicle track (blowup of Figure 5-7 with the 
region of the flyer excluded). 
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Fig. 5.9. Close-up of region of discontinuity following gap in position information. 
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Fig. 5.10. X-position behavior in the vicinity of the discontinuity. 


45 
40 
35 
30 
25 


20 


o 


500 1000 1500 2000 2500 3000 3500 ፦ 


Hie S Il Estimated Heading. 


85 





መላ 
— 
Cn 





— 
md 
> 
a 


— 
E 
DA 


Z Position in Meters Referenced to zinit 


50 52 54 56 58 60 
Time in Seconds After Start of Data Set 


Fig. 5.12. Comparison of measured and estimated Z-position. 


With the aid of the doppler velocimeter, the Kalman filter is able to maintain a 
valid estimate of vehicle parameters during this lengthy gap in position information. The 
key requirements in achieving this are: 


1. The doppler velocimeter is available to provide accurate velocity information 
throughout the data gap. 

2. Prior to losing position information, heading accuracy was sufficient to allow the 
Kalman filter to bridge the gap while still remaining within the window for fix 
acceptance once position data is regained. 
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Chapter 6 


Conclusions 


6.1 Fulfillment of Objectives 


The objective of this thesis was to develop an extended Kalman filter that could 
provide accurate estimates of underwater vehicle position and attitude in real-time. Based 
on the tests of the filter using both simulated and actual data, I believe that the result is a 
qualified success. On the positive side, the Kalman filter was able to process the data fast 
enough to permit real-time processing onboard ship, which would allow major reductions 
in post-cruise processing costs. Additionally, the real-time processing allows those 
conducting sidescan surveys to verify that the desired areas have been mapped by 
checking the navigational estimates. 

A second success 1s that when provided with sufficiently accurate and frequent 
data, the Kalman filter can provide accurate estimates of all vehicle motion and attitude 
changes. Even when gaps occur in the positioning system, the accuracy of the doppler 
velocimeter combined with a good prior estimate of heading allows the filter to maintain 
sufficient accuracy to arrive close to measured position when position data 1s regained. 
Also, the Kalman filter has the ability to estimate gyro bias and compass bias to 
compensate for inaccurate initial heading information and the effects of gyro drift. 
Therefore, the heading estimate becomes increasingly accurate as fix information 1s 
received and the filter is allowed to update over longer periods of time. 

In contrast to the successes, there were also some failures. Many of these can be 
attributed to sensor limitations, especially those of the linear accelerometers contained 
within the IMU package. A constant bias can be compensated for, but in the first data run 
there was an example of erratic accelerometer behavior that threatened the viability of the 
entire estimation process. Only by recognizing this problem and relying on the doppler 
velocimeter entirely could accurate estimates of vehicle position be maintained. While in 
principle the Kalman filter is designed to permit any measurement to be used despite its 


noise characteristics, this application tests that theory due to the varying data rates of the 
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sensors. For a "standard" Kalman filter, all measurements are obtained at every filter 
update step. Therefore, noisy measurements can be made nearly meaningless by assigning 
greater inherent accuracy to less noisy ones. In this case, that is not possible. When the 
only available measurement in the series of integrations leading to position is acceleration, 
the filter 1s forced to use it. Even though its weight 1s initially low, over time these 
measurements have a significant effect on the estimates of velocity and position until other 
sensor data are received. 

A second concern is whether the heading estimates provided by the filter are 
accurate enough to be truly useful for sidescan sonar mapping operation. When scanning 
at long range, even a few tenths of a degree of heading error can be significant. While 
heading accuracy should continue to improve as the filter operates for longer periods, a 
compromise is always necessary when deciding how to weight old measurements. If old 
measurements are weighted too heavily, the filter does not respond to actual changes in 
state variables, such as a change in course. On the other hand, weighting old 
measurements less results in greater filter movement toward new measurements with 
subsequent instability and potentially erroneous changes in bias estimates. 

Finally, the filter as presently constructed 1s of limited utility in estimating vehicle 
motion for a maneuvering vehicle. While, theoretically, control inputs could be added 
fairly easily, the modeling necessary for accurate results is not yet a reality. While most 
current operations involving a thruster-controlled vehicle are limited to a small area and 
can therefore take advantage of a high update rate for position information, future 
advances in vehicle technology promise a long-range, independently-operating 
autonomous vehicle. As constructed here, it is doubtful that the Kalman filter can provide 


sufficient accuracy given the sensors available. 


6.2 Future Work 
There is much work that could be done to improve the performance of this Kalman 
filter estimation process. A few possibilities are as follows. 


First and most obviously, improved sensors would translate directly into improved 


measurements. The primary candidates for such improvements are the linear 
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accelerometers and the gyro. Without gyro stabilization, it is ditficult to improve 
accelerometer stability. However, improvements continue to be made, and it is hoped that 
cost, weight, and power requirements will continue to decline to allow better acceleration 
measurements. As an adjunct to this effort, more testing needs to be performed on the 
accelerometers in use to understand their characteristics and limitations more fully. 
Hopefully, the ring-laser gyro will soon become inexpensive enough for practical use in 
this application to improve heading measurements. Also, the method of obtaining true 
heading used here is only one possibility. Any improvements in true heading accuracy 
would improve the estimates of both heading and heading sensor bias. 

To allow this filter to be useful for such highly maneuverable vehicles as Jason, 
further work needs to be carried out on modeling control inputs to the filter. Additionally, 
if a strapdown IMU package 1s still in use, the addition of the effects of angular velocities 
on linear acceleration and velocities may have to be considered due to the greater angular 
velocities involved. 

Finally, there are always refinements which can be made to improve filter 
performance. One possibility is to add to the state vector estimates for other sensor biases. 
This has the potential for improving all measurements, especially those of the 
accelerometers, which appear to have biases that are always present and frequently 


changing. 
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Appendix 


KFSetup Program 


% This program provides the initial conditions required to run the Kalman 
% filter algorithm. 


DTOR = pi/180;% Conversion of degrees to radians 


% The A matrix propagates the state variables 
ZEISS: 

A(4:6,1:3)2eye(3); 

AMAS IO 12)=eye (8); 


% The G matrix provides the effects of inputs (accelerations and angular 
% velocities) on the state variables 


Gi 7eros( 17.6): 
MESES) =eyeB): 
Gn 124.6) =eye(3), 


% The C matrix is the measurement matrix 

CHDG = [1 -1 031 0-1;1 0 0); % 3x3 matrix used to form heading portions of C matrices 
CR ecye(17)-% Full C matrix when all info available 

Grea 715217) =CHDG: 

CNV = zeros(14,17);% C matrix when no velocity info available 

ከ ከከ 37123) =cye(3): 

CNV (4:11,7:14)=eye(8); 

e 12: T4057) CHDG: 

CNP = zeros(14,17);% C matrix when no position info available 

IDEO eye): 

| ከ ከ ካህ ገሙ ርነር(6): 

ከ 1 1 ሞክር! 1 -CHDOG(IL2.:) 

CNPV = zeros(11,17);% C matrix when neither position nor velocity available 
CAEN E HE = eya): 

CNPV(4:9,9:14)=eye(6); 

CENIO TLIIS T = CHDG(1:2.:); 

% The D matrix is necessary only for generating simulated data 

ID zeros( 170): 

DIN = Zeros 14.6): 

DNP = zeros(14,6); 4 
[ን ከሽ እ = zeros(1 1.6); 

% Error Covariance Matrix for random forcing function 





Q = zeros(6); 

OSA IN Zea: On Setvariance For accelerometers 
Gi: — OXYZ; 

QRPH - (2*DTOR*eye(3)).^2; |. 95 Set variance for angular velocimeters 
Q(4:6,4:6) = QRPH; 

% Error Covariance Matrix for measurements 

ES C (0-] *eye(3).^2; 

SES »5*DIOR*eye(3)).^2; 

RPos z (3*eye(2)).^2; 95 Set variance for acoustic position fixes 
RDepth = 0.142;% Set variance for depth 

RER (05*DTOR*eye(2)).^2; o Set variance for pitch and roll 
RHG = (0.1*DTOR)^2;926 Set variance for heading by gyro 

RHM 2 (.5*DTOR)^2; P Set variance for heading by compass 


% Reference Points 


xinit = (); 
yint = (); 
ün = (); 
zinit = (); 


magvar = -24; J Set initial magnetic compass bias 

gyrovar = -7; % Set gyro bias 

% Initial conditions 

x_0 ACC = [0;0;0]; 

xX_OAR = [0;0;0]; 

ONV E [0050]; Velocities (x,y,z) 

x O0XY z [0-xinit;0-yinit]; £o Position (x,y) 

x_0Z = zinit-);% Position (z) 

x OR = 0*DTOR;@ Roll 

EL = 0FDTOR:% Pitch 

x_OH = (300+gyrovar)* DTOR;®% Heading 

x_0) = 
[x OACC;x 0V;x OXY;x 0Z;:x OAR;x OR;x OP;x OH;gyrovar*DTOR;magvar*DTOR 


ን 


% Initial Error covariance matrix 

P_0 = zeros(17); % Initial P matrix for states 

Pans sy: 1teye(3)).^2; 

P. 0(4:6,4:6) 2 (.02*eye(3)).^2; 96 Initial uncertainty in velocity (m/s) 
DEUS rm (3*eye2)).^2; % Initial uncertainty in position (m) 

P_0(9,9) = ().3^2; % Initial uncertainty in depth (m) 

PAIN 2) (00:5 *DTOR*eye(3)).^2: 

P_0(13:14,13:14) = (1*DTOR *eye(2))^2; % Initial uncertainty in roll and pitch 
P_0(15,15) = (2*DTOR)^2;% Initial uncertainty in heading 1 
P_0(16,16) = 2*DTOR)Y’2;% Set initial gyro bias variance 

P_0(17,17) = (2*DTOR)^2;% Set initial magnetic compass bias variance 
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w = zeros(6,1);% random forcing function 

g = -9.8;% gravity vector 

% Set IMU biases to be subtracted from raw data (units of g for acc, degrees/s for 
7o angular rates) 


xdeebias -— 0; 
yaccbias= 0; 
zaccbias = 0; 


rrbias = 0; 
prbias = 0; 
yrbias = 0; 
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KFRun 


%odt = 1/24/3600/2; % Gives desired kalman filter update time step of 0.5 seconds 
t0— tnit; % Set initial time for filter for calculating time steps 

t = [(); 

tie = t0); % Time of last fix 


kesi; % Index for state estimate file 
im (): 

mm z (; 

qq = l; 


Z= Zeros(3, 1; 

22 cros(3,1) 

Posmat = zeros(4,2); 

fixcount = (); 

SWAcheck = 0;% Set Counters to count lines of each type of data in filter step 
PAScheck = (): 

PVScheck = 0); 

Meeec heck = (); 

FixedSim2=zeros(7200,18); % Matrix to hold processed raw data 
TempIn=[]; 

EstSim2 = []; 

rejfix =0; % Counter to keep track of rejected fixes 

Xp =x_0; % Set initial condition for estimated states 

Pp = P_0; % Set initial condition for error covariance matrix 
while nn < 7200; % Setup loop to read all data from file 


% The following loop ensures both SWA and PAS data are available to the filter 
while SW Acheck == () I PAScheck == 0) 

mm = mm+l; 

nn =nn+l; 

TempIn(mm,:) = SIM(nn,:); % SIM is data file 

TempIn(mm,1) 2 TempIn(mm,l)-tinit; 

[M,N] 2 size(TempIn): 

t= TempIn(M,1); 

% This loop reads the next dt worth of data from the file. For real-time 
% processing, use same principle to read dt worth of data into Templin. 

while TempIn(mm, 1)-t0 <= dt; % Check to see if have dt worth of data 
nn =nn+l; 

mm =mm+l; 

TempIn(mm,:) 2 SIM(nn,:); 

TempIn(mm,1) 2 TempIn(mm,l)-tinit; 

[M,N] = size(TempIn); 

t= Tempin(M.1); 
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end 


% This loop checks if both SWA and PAS data 1s present. If the data does not 
% contain at least one sample of both SWA and PAS data, another dt worth will 
% be added prior to proceeding. 

[0፲ 1 = ባባ፡እብ 

if TempIn(1,2) «10631 | TempIn(i,2) > 10633 

SW Acheck = SWAcheck+1; 

else 

TempIn(1,2:7) = zeros(1,6); % set bogus data to zero 
end 

if TempIn(1,13) <10631 | TempIn(1,13) > 10633 
PAScheck = PAScheck+1; 

TempIn@, 13) = zinit-TempIn(i, 13); 

TempIn(i,14:15) = TempIn(1,14:15).*DTOR; 

if (360-TempIn(1,16))<75 | TempIn(1,16)<75 

if TempIn(1,16)>270 
Templn(1,16)2TemplIn(1,16)-360; 

else 

end 

else 

end 

TempIn(1,16) = TempIn(1,16)*DTOR; 

if (360-TempIn(1,17))<75 | TempIn(1,17)<75 

if TempIn(1,17)>270 
TempIn(i,17)=TempInG@,17)-360; 

else 

end 

else 

end 

TempIn(1,17) = TempIn(1,17)*DTOR; 

else 

TempIn(1,13:17) = zeros(1,5); % set bogus data to zero 
end 

it TempIn(1,8) «10631 | TempIn(1,8) » 10633 
PVSchħheck [ችህ ስ ርከርርኪ 1-1: 

else 

TempiIn(1,8: 10) 2 zeros(1,3); % set bogus data to zero 
TempIn(i,18) = 0; 

end 

it TempIn(1,11) «10631 | TempIn(1,1 1) » 10633 
LBLcheck = LBLcheck+1; 

Templn(1,1 1) 2 TempIn(1,1 1)-xinit; 

TemplIn(1,12) 2 TemplIn(1,12)-yinit; 

else 

TemplIn(1,11:12) 2 zeros(1,2); 9» set bogus data to zero 
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end 

end 

qq = M+; 

end 

FixedSim2((nn+1-M):nn+1-1,:)=Templn; 

% Now we will provide the estimates of x4(t+1lt) and P(t+1It). These are done 
% now rather as the last step in the filter propagation due to the vanable 

% time step. Now that we know the time step, we can more accurately project 
% the step estimates into the future. 

DT = (t-t0)*24*3600; % This is the length of the current filter time step. 


[Xm,Pm] = tullktpred(A,G,w,Q,Xp,Pp,DT); 


% We now have enough data to run the filter. For data with more than one time 
% sample’s worth present, the data for the whole time step is averaged. 

% Also, any suspected bias 1s removed. The inputs are converted into the proper 
% units tor filter use. Also, the data is checked to see if LBL and/or doppler 

% data is available, which will determine which version of the filter to use. 

% Theretore, the input to the kalman filter will have only one line of 

% averaged data, which will always include SWA and PAS data, and may also 
% have LBL and/or doppler data. 

zl(1) 2 (((sum(TempIn(:,2))/5W Acheck))-xacchias)*(-g)+((sin(Xp(13)))*g); 
2 UPC ,3)/ 8W Acheck))-yaccbias) *(-g)-((sin(Xp(14)))*g); 
210)) (((sum(TempIn(:,4))/SW Acheck))-zaccbias)*(-g)- 
((cos(Xp(13))*cos(Xp(14)))*g): 

z2(1) = (((sum(TempIn(:,5))/SW Acheck))-rrbias)* DTOR; 

z2(2) = (((sum(TempIn(:,6))/S W Acheck))-prbias)*DTOR; 

z2(3) 2 (((sum(TempIn(:,7)/ SW Acheck))-yrbias) * DTOR; 

%zl=zeros(3,1); % Used only if not using actual accelerometer measurements 
izz =zer 0S (31); 


% One other requirement is to check for position flyers. If a position update 
% 18 bogus, it will be ignored and the filter run without it. A position update 
% will be compared with the current filter estimate of position. If they differ 
% by more than five times the std deviation of the position uncertainty, the 
% position update will be ignored. 

it DBIL check «9 

tcheck = (t-tfix): 

Pcheckx = sum(TempIn(:,11))/LBLcheck; 

Pchecky = sum(TempIn(:,12))/LBLcheck; 

errest = sqrt(((Pcheckx-Xp(7))*2)+(Pchecky-X p(&))*2); 

if errest > (LO*RPos(1,1))+(0.05*tcheck) 

rejfix = rejfix+1; 


LBLcheck =(): . 
else 
tk: 


907 








end 
end 


% The first case is when all data is available. True heading will only be 
% available after five fixes have been obtained. 

% Angular measurements have already been converted to radians using DTOR. 
if LBLcheck ~= 0 & PVScheck ~= 0 

fixcount = fixcount+1; 

Ae Os ll 

ZE =z, 

Z(4) 2 sum(TemplIn(:,8)/PVScheck; 

Z(5) 2 sum(Templn(:,9)/ PVScheck; 

Z(6) 2 sum(TemplIn(:,10)PVScheck; 

xiu — sumtltempln(:; 1 DVISBLEcheck; 


ZUM XLBL; 
yLBL= sum(TempIn(:,12))/LBLcheck; 
Z(8) = yLBL; 


Z(9) = sum(TempIn(:, 13)/PAScheck; 
ZN) = z2; 

Z(13) = sum(Templn(:,14)/PAScheck; 
Z(14) = sum(Templn(:,15)/PAScheck; 
ghdg = sum(TempIn(:,16))/PAScheck; 
if ghdg«0 

ghdg = ghdg+(2*pi); 


else 

end 

Zils) =ehde, 

mhdg = sum(TempIn(:,17))/PAScheck; 
if mhdg«0 

mhdg = mhdg+(2* pi); 

else 

end 

Z(16) = mhdg; 


R= zeros( l7), 

R(1:3,1:3) = RACC; 

R(4:6,4:6) = eye(3).*((sum(TempIn(:, 18))/PVScheck)*2): 
RS. 6 Ree 

ROY) = R Depth; 

RGO 12.1042) = RAR; 

R(13:14,13:14) = RPR: 

R@5,15) = RHG; 

R(16,16) = RHM; 

GER; 


% Update the matrix containing the last four position fixes 
Posmi — Posmat(2,:); 
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ክበብን Posmat(3;:); 
Posmat(3,:) = Posmat(4,:); 
Posmat(4,:) = {xLBL,yLBL]; 
If fixcount«4 

= 7, 1:16); 

R =R(1:16.1:16); 

@ = ORCI.) 

else 


% Now, Calculate true heading using a least-squares fit to the last four fixes 
| ePosmater.!)-1,Posmat(2,1);1,Posmat(3,1):1,xLBL]; 
B = [Posmat(1,2);Posmat(2,2);Posmat(3,2); yLBL]; 

eet = Inv *L)*L’*B; 

if bfit(2)>=0 & yLBL > Posmat(1,2) 

thdg = (pi/2)-atan(bfit(2)); 

elseit btit(2)«0 & yLBL « Posmat(1,2) 

thdg = (pi/2)-atan(bfit(2)); 

else 

thdg = (3*pi/2)-atan(bfit(2)); 

end 

Za» -shdg: 

% Calculate variance of this heading measurement 

[HV] = hvar(Posmat,thdg); 

fei 7,17) =(HV]): 

end 


% The second case 1s when SWA, PAS, and LBL data is available, but no doppler. 
elsemeumlcnctk ~=() & PVScheck == () 


fixcount = fixcount+1; 
Z = zeros( 14,1); 


2:3) = 21: 

xLBL 2 sum(Templn(:,1 1)/LBLcheck; 
A@)= xLBL; 

yLBL = sum(Temp!In(:,12))/LBLcheck; 
ZO) = yUBL: 

Z(6) = sum(TempIn(:,13))/PAScheck; 
LEIDO 


Z(10) = sum(Templn(:,14)/PAScheck; 

Z(11) = sum(TempIn(:,15))/PAScheck; 

ghdg 2 sum(TempIn(:,16)/PAScheck; 

it ghdg«O 

ghdg = ghdg+(2*p1); 

else - 
end 

Z112 phdg: 
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mhdg 2 sum(TemplIn(:,17)/ PAScheck; 

if mhdg«0 

mhdg = mhdg+(2*pi); 

else 

end 

Z(13) 2 mhdg; 

R = zeros(14,14); 

R(1:3,1:3) = RACC; 

R(4:5,4:5) = RPos; 

R(6,6) 2 R Depth; 

Ri@e7,7:9) = RAR; 

Rees 1 10:11) =RPR: 

Re 12) =RHG:; 

R(13,13) = RHM; 

C=CNV; % Update the matrix containing the last four position fixes 
Posmat(1,:) 2 Posmat(2,:); 

Fosmat(2,:) = Posmat(3,:): 

Posmat(3,:) 2 Posmat(4,:); 

Posmat(4,:) = [xLBL,yLBL]; 

If fixcount«4 

2 7001.13) 

hea iat 124.1213); 

ሽጨ (ብህ!!!) 

else 

% Calculate true heading 

eer osinan 1). 1 Posmat(2,1);1,.Posmat(3,!);1,xLBL}; 
fb — [Posmaty,2);Posmat(2,2);Posmat(3,2);yLBL]; 
[በበር በከ 1 LL *B; 

if bfit(2)>=0 & yLBL > Posmat(1,2) 

thdg = (pi/2)-atan(bfit(2)); 

elseif bhit(2)<0 & yLBL « Posmat(1,2) 

thdg = (pi/2)-atan(bfit(2)); 

else 

thdg 2 (3*pi/2)-atan(bfit(2)); 

end 

Z2 - thdg: 

% Calculate variance of this heading measurement 
[HV] = hvar(Posmat,thdg); 

Rigas [HV]; 

end 


% The third case is when SWA, PAS, and doppler data is available, but no LBL. 
een EBEcheck == 0 & PVScheck ~=() 


Z = zeros(14,1); 
ZA em 
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Z(4) = sum(Templn(:,8)/PVScheck; 
Z(5) = sum(TempIn(:,9))/PVScheck; 
Z(6) = sum(TempIn(:,10))/PVScheck; 
Z(7) = sum(TempIn(:,13))/PAScheck; 
ዘከሮ 72; 

Z(11) = sum(TempIn(:,14))/PAScheck; 
(12) — sum(Tempin(,15)/PAScheck; 
ghdg = sum(TempIn(:,16))/PAScheck; 
if ghdg«0 

ghdg = ghdg+(2*p1); 

else 

end 

AS) = ende; 

mhdg = sum(TempIn(:,17))/PAScheck; 
if mhdg<() 

mhdg = mhdg+(2*pi); 

else 

end 

Z(14) = mhdg; 

R = zeros( 14); 

R(1:3,1:3) = RACC; 

R(4:6,4:6) 2 eye(3).*((sum(TempIn(:,18)/PVScheck)^2); 
ከ ከ D -RDepth; 

R(8:10,8:10) = RAR; 

Reel 2,11:12)=RPR; 

RX 18413) = RHG; 

R(14,14) =RHM; 

S= CNP: 


% The last case 1s when only SWA and PAS data is available. 


else 

Z= 7eros( 1 1,1): 

AA 

Z(4) = sum(TempIn(:,13))/PAScheck; 
LASA MED 

Z(8) = sum(TempIn(:,14))/PAScheck; 
Z(9) = sum(TempIn(:,15))/PAScheck; 
ghdg 2 sum(TempIn(:,16)/ PAScheck; 
if ghdg«0 

ghdg = ghdg+(2*pi); 

else 

end 

Z(10) = ghdg; 

mhdg = sum(TempIn(:,17))/PAScheck; 
if mhdg«0 
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mhdg = mhdg+(2*p1); 
else 

end 

Z(11) = mhdg; 

R= zeros(1 1); 
R(1:3,1:3) = RACC; 
R(4,4) = RDepth; 
RO 5:7) — RAK: 
R(8:9,8:9) = RPR; 
R(10,10) = RHG; 
R(11,11) = RHM; 
(x NDV: 

end 

% Now the filter can finally be run using KFeale. 


[Xp,Pp]= fullkftcalc(C,R,Xm,Pm,Z); 
EstS1m2(kk,1:18) 2 [Xp'.t]; % Store state estimates with time stamp nn 
SW Acheck z 0); 

PAScheck z 0; 

PBEcheck =0; 

Pay scheck z 0: 

Mis 

Wet: 

ባባ = 1; 

Tempn =]; 

kk = kk+1; 

pp-l; 

end 
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KF pred 


% The function kfpred provides the estimates of x^(t-1lt) and P(t4 Ilt). 

Fo lt must be done out of the normal order for a Kalman filter since the time 
% Step is unknown until the next block of data is processed. 

% The format is: [Xm,Pm]=kfpred(A,G,U,Q,xcurr,P,t) 

% tis the time stamp for the current update step. 
function[Xm,Pm]=ktpred(A,G,U,Q,xcurr,P,t) 

Heade = xcurm(]5); 

pitch = xcurr(14); 

[ወ = ፈር (1.3); 

dheadg = (2* pi - headg); % Change axes 

pcos = cos(pitch); 

psin = sin(pitch); 

rcos = cos(roll); 

rsin = sin(roll); 

hcos = cos(dheadg); 

hsin = sin(dheadg); 

A(7,4) = rcos*hcos; 

A(7,5) 2 psin*rsin*hcos - pcos*hsin; 

A(7,6) = pcos*rsin*hcos + psin*hsin; 

A(8,4) = hsin*rcos; 

A(8,5) = psin*rsin*hsin + pcos*hcos; 

A(8,6) = pcos*rsin*hsin - psin*hcos; 

A(9,4) 2 -rsin; A(9,5) 2 psin*rcos; 

A(9,6) 2 pcos*rcos; 

[Phi, Gamma] = c2d(A,G.,t); 

Xm = Phi*xcurr + Gamma*U; % Xm = x^(t+lit) 

%Pm = 1.0168*Phi*P*Phi’ + Gamma*Q*Gamma’; % Use for maneuvering vehicle 
Pm = 1.001*Phi*P*Phi' - Gamma*Q*Gamma'; % Use when have position data gaps or 
no maneuvering 
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FullKF calc 


% The function FullKFCalc implements the kalman filter for full data sets 
% The format is: [Xp,Pp]=fullkfcalc(C,R,Xm,P,Z) 
% tis the time stamp for the current update step. 
function[Xp,Pp]=fullkfcalc(C,R,Xm,P,Z) 

while Xm(15) < 0 

Xm(15) = Xm(15)+(2*p1); 

end 

while Xm(15) > 2* pi 

Xm(15) = Xm(15)-(2*pi); 

end 

while Xm(16)>p1 

Xm(16)=Xm(16)-(2* pi); 

end 

while Xm(16)<(-p1) 

Xm(16)=Xm(16)+(2*pi); 

end 

while Xm(17)>pi 

Xm(17)=Xm(17)-(2*pi); 

end 

while Xm(17)<(-pi) 

Am(17)=Xm(17)+(2* pi); 


end 

Ze = C*Xm; % Le = y(tlt-1) 

Ze 96 12 y(t)-y^(tt- 1) 

“=C CPC +R; % v = covariance matrix of innovations 
Hc uno 

ip in El: Fox ESOS TO 


while Xp(15) « 0 
Xp(15) 2 Xp(15)+(2* pi); 
end 

while Xp(15) » 2*pi 
Xp(15) = Xp(15)-(2*pi): 
end 

while Xp(16)>p1 
Xp(16)=Xp(16)-(2* pi); 
end 

while Xp(16)<(-p1) 
Xp(16)=Xp(16)+(2*pi); 
end 

while Xp(17)>p1 
Xp(17)=Xp(17)-Q*pi); 





end 

while Xp(17)<(-pi) 

Xp(17)=Xp(17)+(2*pi); 

end 

temp = eye(17)-(k*C); 

Pp =temp*P*temp +k*R*k’ ; Ap = RUD 
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HVar 


% The function hvar provides the variance of the true heading 
% measurement. 

% The format is [HV] = hvar(Posmat,thdg) 
function[HV] = hvar(Posmat,thdg) 
tempvar = 0); 

Elemp=7eros 3.1); 

fore = 123: 

LL =[1,Posmat(i,1);1,Posmat(i+1,1)}; 

BB = [Posmat(i,2);Posmat(i+1,2)]; 

ኮከ 1 ከ Eins LE FLE)TLL'*BB; 

if bbfit(2)>=0 & BB(2)>BB(1) 

temphdg = (p1/2)-atan(bbfit(2)); 

elseif bbfit(2)<0 & BB(2)<BB(1) 

temphdg z (pi/2)-atan(bbfit(2)); 

else temphdg = (3*pi/2)-atan(bbfit(2)); 


end 

Htemp(1) 2 temphdg; 
end 

lor 1:3: 


if Htemp(1)-thdg»pi 

HVar(1) 2 ((Htemp(1)-(2*p1)-thdg)^2; 
elseif thdg-Htemp(i)>pi 

HVar(1) = ((thdg-(2*pi))-Htemp(1))^2; 
else 

HVar(1) = (Htemp(i)-thdg)^2; 

end 

end 

HV = (sum(HVar))/3; 
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